manta: batching and timestamp issues
Signed-off-by: Mark Salyzyn <salyzyn@google.com>
Signed-off-by: Anna Si <asi@invensense.com>
Bug: 18958411
Change-Id: Ie06912843e82811a6728cf0a67ce1cf801efa997
diff --git a/60xx/libsensors_iio/Android.mk b/60xx/libsensors_iio/Android.mk
index a5dbee2..3b95c86 100644
--- a/60xx/libsensors_iio/Android.mk
+++ b/60xx/libsensors_iio/Android.mk
@@ -71,6 +71,10 @@
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
LOCAL_PRELINK_MODULE := false
include $(BUILD_SHARED_LIBRARY)
@@ -87,6 +91,8 @@
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
@@ -98,5 +104,20 @@
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
endif # !TARGET_SIMULATOR
diff --git a/60xx/libsensors_iio/MPLSensor.cpp b/60xx/libsensors_iio/MPLSensor.cpp
index f1ec9ea..9b9534d 100644
--- a/60xx/libsensors_iio/MPLSensor.cpp
+++ b/60xx/libsensors_iio/MPLSensor.cpp
@@ -1604,6 +1604,10 @@
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);
@@ -1904,9 +1908,10 @@
((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 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 @@
}
}
+/** 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 result, first_error;
int kk;
- int mode;
#ifdef INV_PLAYBACK_DBG
if (inv_data_builder.debug_mode == RD_RECORD) {
@@ -904,22 +1015,22 @@
}
#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 @@
#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 @@
* 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 @@
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 @@
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 @@
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 @@
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 @@
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 @@
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
