From b40e8f18f4cd2e4c6e365383094268ceae837b9f Mon Sep 17 00:00:00 2001 From: Anna Si Date: Fri, 13 Feb 2015 12:48:44 -0800 Subject: manta: batching and timestamp issues Signed-off-by: Mark Salyzyn Signed-off-by: Anna Si Bug: 18958411 Change-Id: Ie06912843e82811a6728cf0a67ce1cf801efa998 --- 60xx/libsensors_iio/Android.mk | 21 ++++ 60xx/libsensors_iio/MPLSensor.cpp | 7 +- .../software/core/mllite/data_builder.c | 127 +++++++++++++++++++-- .../software/core/mllite/data_builder.h | 7 ++ .../software/core/mllite/hal_outputs.c | 25 ++-- .../software/core/mllite/hal_outputs.h | 4 + 6 files changed, 173 insertions(+), 18 deletions(-) diff --git a/60xx/libsensors_iio/Android.mk b/60xx/libsensors_iio/Android.mk index 5d936a2..fd8e472 100644 --- a/60xx/libsensors_iio/Android.mk +++ b/60xx/libsensors_iio/Android.mk @@ -69,6 +69,10 @@ LOCAL_SHARED_LIBRARIES += libmllite LOCAL_SHARED_LIBRARIES += libmplmpu LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl LOCAL_CPPFLAGS += -DLINUX=1 +# Experimental +ifeq ($(BOARD_INV_LIBMLLITE_FROM_SOURCE),true) +LOCAL_CPPFLAGS += -DLIBMLLITE_FROM_SOURCE +endif include $(BUILD_SHARED_LIBRARY) @@ -84,6 +88,8 @@ OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES) LOCAL_STRIP_MODULE := true include $(BUILD_PREBUILT) +# Experimental +ifneq ($(BOARD_INV_LIBMLLITE_FROM_SOURCE),true) include $(CLEAR_VARS) LOCAL_MODULE := libmllite LOCAL_SRC_FILES := libmllite.so @@ -95,3 +101,18 @@ LOCAL_MODULE_PATH := $(TARGET_OUT)/lib OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES) LOCAL_STRIP_MODULE := true include $(BUILD_PREBUILT) +else +include $(CLEAR_VARS) +LOCAL_CFLAGS += -DANDROID_JELLYBEAN +LOCAL_CFLAGS += -DLINUX=1 +LOCAL_MODULE := libmllite +LOCAL_SRC_FILES := $(call all-c-files-under, software/core/mllite) +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux +LOCAL_MODULE_OWNER := invensense +LOCAL_MODULE_PATH := $(TARGET_OUT)/lib +LOCAL_SHARED_LIBRARIES := liblog +include $(BUILD_SHARED_LIBRARY) +endif diff --git a/60xx/libsensors_iio/MPLSensor.cpp b/60xx/libsensors_iio/MPLSensor.cpp index 0aa6556..031ae6e 100644 --- a/60xx/libsensors_iio/MPLSensor.cpp +++ b/60xx/libsensors_iio/MPLSensor.cpp @@ -1588,6 +1588,10 @@ int MPLSensor::update_delay() { inv_set_gyro_sample_rate(mplGyroRate); inv_set_accel_sample_rate(mplAccelRate); inv_set_compass_sample_rate(mplCompassRate); +#ifdef LIBMLLITE_FROM_SOURCE + inv_set_linear_acceleration_sample_rate(rateInus); + inv_set_gravity_sample_rate(rateInus); +#endif /* TODO: Test 200Hz */ // inv_set_gyro_sample_rate(5000); @@ -1884,9 +1888,10 @@ int MPLSensor::readEvents(sensors_event_t* /*data*/, int /*count*/) { ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))); #endif - if (rsize < (nbyte - 8)) { + if (rsize != nbyte) { LOGE("HAL:ERR Full data packet was not read. rsize=%zd nbyte=%d sensors=%d errno=%d(%s)", rsize, nbyte, sensors, errno, strerror(errno)); + rsize = read(iio_fd, rdata, sizeof(mIIOBuffer)); return -1; } diff --git a/60xx/libsensors_iio/software/core/mllite/data_builder.c b/60xx/libsensors_iio/software/core/mllite/data_builder.c index 48993bc..0aa418d 100644 --- a/60xx/libsensors_iio/software/core/mllite/data_builder.c +++ b/60xx/libsensors_iio/software/core/mllite/data_builder.c @@ -62,6 +62,7 @@ struct inv_data_builder_t { struct process_t process[INV_MAX_DATA_CB]; struct inv_db_save_t save; int compass_disturbance; + int mode; #ifdef INV_PLAYBACK_DBG int debug_mode; int last_mode; @@ -239,6 +240,117 @@ void inv_set_accel_sample_rate(long sample_rate_us) } } +/** Pick the smallest non-negative number. Priority to td1 on equal +* If both are negative, return the largest. +*/ +static int inv_pick_best_time_difference(long td1, long td2) +{ + if (td1 >= 0) { + if (td2 >= 0) { + if (td1 <= td2) { + // td1 + return 0; + } else { + // td2 + return 1; + } + } else { + // td1 + return 0; + } + } else if (td2 >= 0) { + // td2 + return 1; + } else { + // Both are negative + if (td1 >= td2) { + // td1 + return 0; + } else { + // td2 + return 1; + } + } +} + +/** Returns timestame based upon a raw sensor, and returns if that sample has a new piece of data. +*/ +static int inv_raw_sensor_timestamp(int sensor_number, inv_time_t *ts) +{ + int status = 0; + switch (sensor_number) { + case 0: // Quat + *ts = sensors.quat.timestamp; + if (inv_data_builder.mode & INV_QUAT_NEW) + if (sensors.quat.timestamp_prev != sensors.quat.timestamp) + status = 1; + return status; + case 1: // Gyro + *ts = sensors.gyro.timestamp; + if (inv_data_builder.mode & INV_GYRO_NEW) + if (sensors.gyro.timestamp_prev != sensors.gyro.timestamp) + status = 1; + return status; + case 2: // Accel + *ts = sensors.accel.timestamp; + if (inv_data_builder.mode & INV_ACCEL_NEW) + if (sensors.accel.timestamp_prev != sensors.accel.timestamp) + status = 1; + return status; + case 3: // Compass + *ts = sensors.compass.timestamp; + if (inv_data_builder.mode & INV_MAG_NEW) + if (sensors.compass.timestamp_prev != sensors.compass.timestamp) + status = 1; + return status; + default: + *ts = 0; + return 0; + } + return 0; +} + +/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination. +* It does this by finding a raw sensor that has the closest sample rate that is at least as +* often desired. It also returns if that raw sensor has a new piece of data. +* Priority is Quaternion-6axis, Quaternion 3-axis, Gyro, Accel +* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise. +*/ +int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts) +{ + long td[2]; + int idx; + + if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) { + // Sensor number is 0 (Quat) + return inv_raw_sensor_timestamp(0, ts); + } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) { + return 0; // Accel must be on or 6-axis quat must be on + } + + // At this point, we know accel is on. Check if 3-axis quat is on + td[0] = sample_rate_us - sensors.quat.sample_rate_us; + td[1] = sample_rate_us - sensors.accel.sample_rate_us; + if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) { + idx = inv_pick_best_time_difference(td[0], td[1]); + idx *= 2; + // 0 = quat, 3=accel + return inv_raw_sensor_timestamp(idx, ts); + } + + // No Quaternion data from outside, Gyro must be on + if ((sensors.gyro.status & INV_SENSOR_ON) == 0) { + return 0; // Gyro must be on + } + + td[0] = sample_rate_us - sensors.gyro.sample_rate_us; + idx = inv_pick_best_time_difference(td[0], td[1]); + idx *= 2; // 0=gyro 2=accel + idx++; + // 1 = gyro, 3=accel + return inv_raw_sensor_timestamp(idx, ts); +} + /** Set Compass Sample rate in micro seconds. * @param[in] sample_rate_us Set Gyro Sample rate in micro seconds. */ @@ -895,7 +1007,6 @@ inv_error_t inv_execute_on_data(void) { inv_error_t result, first_error; int kk; - int mode; #ifdef INV_PLAYBACK_DBG if (inv_data_builder.debug_mode == RD_RECORD) { @@ -904,22 +1015,22 @@ inv_error_t inv_execute_on_data(void) } #endif // Determine what new data we have - mode = 0; + inv_data_builder.mode = 0; if (sensors.gyro.status & INV_NEW_DATA) - mode |= INV_GYRO_NEW; + inv_data_builder.mode |= INV_GYRO_NEW; if (sensors.accel.status & INV_NEW_DATA) - mode |= INV_ACCEL_NEW; + inv_data_builder.mode |= INV_ACCEL_NEW; if (sensors.compass.status & INV_NEW_DATA) - mode |= INV_MAG_NEW; + inv_data_builder.mode |= INV_MAG_NEW; if (sensors.temp.status & INV_NEW_DATA) - mode |= INV_TEMP_NEW; + inv_data_builder.mode |= INV_TEMP_NEW; if (sensors.quat.status & INV_QUAT_NEW) - mode |= INV_QUAT_NEW; + inv_data_builder.mode |= INV_QUAT_NEW; first_error = INV_SUCCESS; for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { - if (mode & inv_data_builder.process[kk].data_required) { + if (inv_data_builder.mode & inv_data_builder.process[kk].data_required) { result = inv_data_builder.process[kk].func(&sensors); if (result && !first_error) { first_error = result; diff --git a/60xx/libsensors_iio/software/core/mllite/data_builder.h b/60xx/libsensors_iio/software/core/mllite/data_builder.h index 9aa46e6..c8c18cf 100644 --- a/60xx/libsensors_iio/software/core/mllite/data_builder.h +++ b/60xx/libsensors_iio/software/core/mllite/data_builder.h @@ -26,6 +26,10 @@ extern "C" { #define INV_TEMP_NEW 8 /** This is a new sample of quaternion data */ #define INV_QUAT_NEW 16 +/** Set if quaternion is 6-axis from DMP */ +#define INV_QUAT_6AXIS 1024 +/** Set if quaternion is 3-axis from DMP */ +#define INV_QUAT_3AXIS 4096 /** Set if the data is contiguous. Typically not set if a sample was skipped */ #define INV_CONTIGUOUS 16 @@ -98,6 +102,7 @@ struct inv_quat_sensor_t { * INV_CALIBRATED_SET if calibrated data has been solved for */ int status; inv_time_t timestamp; + inv_time_t timestamp_prev; long sample_rate_us; long sample_rate_ms; }; @@ -225,6 +230,8 @@ int inv_get_compass_disturbance(void); inv_error_t inv_get_gyro_orient(int *orient); inv_error_t inv_get_accel_orient(int *orient); +// internal +int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts); #ifdef __cplusplus } diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.c b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c index 7cdca59..caa1db7 100644 --- a/60xx/libsensors_iio/software/core/mllite/hal_outputs.c +++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c @@ -42,10 +42,22 @@ struct hal_output_t { int nine_axis_status; inv_biquad_filter_t lp_filter[3]; float compass_float[3]; + long linear_acceleration_sample_rate_us; + long gravity_sample_rate_us; }; static struct hal_output_t hal_out; +void inv_set_linear_acceleration_sample_rate(long sample_rate_us) +{ + hal_out.linear_acceleration_sample_rate_us = sample_rate_us; +} + +void inv_set_gravity_sample_rate(long sample_rate_us) +{ + hal_out.gravity_sample_rate_us = sample_rate_us; +} + /** Acceleration (m/s^2) in body frame. * @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it * should return a vector of magnitude near 9.81 m/s^2 @@ -85,8 +97,9 @@ int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, inv_time_t * timestamp) { long gravity[3], accel[3]; + inv_time_t timestamp1; - inv_get_accel_set(accel, accuracy, timestamp); + inv_get_accel_set(accel, accuracy, ×tamp1); inv_get_gravity(gravity); accel[0] -= gravity[0] >> 14; accel[1] -= gravity[1] >> 14; @@ -95,7 +108,7 @@ int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, values[1] = accel[1] * ACCEL_CONVERSION; values[2] = accel[2] * ACCEL_CONVERSION; - return hal_out.nine_axis_status; + return inv_get_6_axis_gyro_accel_timestamp(hal_out.linear_acceleration_sample_rate_us, timestamp); } /** Gravity vector (m/s^2) in Body Frame. @@ -109,19 +122,13 @@ int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, inv_time_t * timestamp) { long gravity[3]; - int status; *accuracy = (int8_t) hal_out.accuracy_quat; - *timestamp = hal_out.nav_timestamp; inv_get_gravity(gravity); values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION; values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION; values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION; - if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA)) - status = 1; - else - status = 0; - return status; + return inv_get_6_axis_gyro_accel_timestamp(hal_out.gravity_sample_rate_us, timestamp); } /* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.h b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h index 85e88f3..41b72c6 100644 --- a/60xx/libsensors_iio/software/core/mllite/hal_outputs.h +++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h @@ -38,6 +38,10 @@ extern "C" { inv_error_t inv_start_hal_outputs(void); inv_error_t inv_stop_hal_outputs(void); + // Set data rates for virtual sensors + void inv_set_linear_acceleration_sample_rate(long sample_rate_us); + void inv_set_gravity_sample_rate(long sample_rate_us); + #ifdef __cplusplus } #endif -- cgit v1.2.3