am eb77dbf0: Merge "Revert "manta: batching and timestamp issues""

* commit 'eb77dbf00ed67a8e12ab376921300883f6975ce2':
  Revert "manta: batching and timestamp issues"

6 files changed, 18 insertions, 167 deletions

diff --git a/60xx/libsensors_iio/Android.mk b/60xx/libsensors_iio/Android.mk
index 874f63e..5d936a2 100644
--- a/60xx/libsensors_iio/Android.mk
+++ b/60xx/libsensors_iio/Android.mk
@@ -84,8 +84,6 @@ 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
@@ -97,18 +95,3 @@ 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 610069c..0aa6556 100644
--- a/60xx/libsensors_iio/MPLSensor.cpp
+++ b/60xx/libsensors_iio/MPLSensor.cpp
@@ -1588,8 +1588,6 @@ int MPLSensor::update_delay() {
         inv_set_gyro_sample_rate(mplGyroRate);
         inv_set_accel_sample_rate(mplAccelRate);
         inv_set_compass_sample_rate(mplCompassRate);
-        inv_set_linear_acceleration_sample_rate(rateInus);
-        inv_set_gravity_sample_rate(rateInus);
 
         /* TODO: Test 200Hz */
         // inv_set_gyro_sample_rate(5000);
@@ -1886,10 +1884,9 @@ int MPLSensor::readEvents(sensors_event_t* /*data*/, int /*count*/) {
             ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
 #endif
 
-    if (rsize != nbyte) {
+    if (rsize < (nbyte - 8)) {
         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 0aa418d..48993bc 100644
--- a/60xx/libsensors_iio/software/core/mllite/data_builder.c
+++ b/60xx/libsensors_iio/software/core/mllite/data_builder.c
@@ -62,7 +62,6 @@ 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;
@@ -240,117 +239,6 @@ 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.
 */
@@ -1007,6 +895,7 @@ 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) {
@@ -1015,22 +904,22 @@ inv_error_t inv_execute_on_data(void)
     }
 #endif
     // Determine what new data we have
-    inv_data_builder.mode = 0;
+    mode = 0;
     if (sensors.gyro.status & INV_NEW_DATA)
-        inv_data_builder.mode |= INV_GYRO_NEW;
+        mode |= INV_GYRO_NEW;
     if (sensors.accel.status & INV_NEW_DATA)
-        inv_data_builder.mode |= INV_ACCEL_NEW;
+        mode |= INV_ACCEL_NEW;
     if (sensors.compass.status & INV_NEW_DATA)
-        inv_data_builder.mode |= INV_MAG_NEW;
+        mode |= INV_MAG_NEW;
     if (sensors.temp.status & INV_NEW_DATA)
-        inv_data_builder.mode |= INV_TEMP_NEW;
+        mode |= INV_TEMP_NEW;
     if (sensors.quat.status & INV_QUAT_NEW)
-        inv_data_builder.mode |= INV_QUAT_NEW;
+        mode |= INV_QUAT_NEW;
 
     first_error = INV_SUCCESS;
 
     for (kk = 0; kk < inv_data_builder.num_cb; ++kk) {
-        if (inv_data_builder.mode & inv_data_builder.process[kk].data_required) {
+        if (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 c8c18cf..9aa46e6 100644
--- a/60xx/libsensors_iio/software/core/mllite/data_builder.h
+++ b/60xx/libsensors_iio/software/core/mllite/data_builder.h
@@ -26,10 +26,6 @@ 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
@@ -102,7 +98,6 @@ 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;
 };
@@ -230,8 +225,6 @@ 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 caa1db7..7cdca59 100644
--- a/60xx/libsensors_iio/software/core/mllite/hal_outputs.c
+++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c
@@ -42,22 +42,10 @@ 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

@@ -97,9 +85,8 @@ 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, &timestamp1);

+    inv_get_accel_set(accel, accuracy, timestamp);

     inv_get_gravity(gravity);

     accel[0] -= gravity[0] >> 14;

     accel[1] -= gravity[1] >> 14;

@@ -108,7 +95,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 inv_get_6_axis_gyro_accel_timestamp(hal_out.linear_acceleration_sample_rate_us, timestamp);

+    return hal_out.nine_axis_status;

 }

 

 /** Gravity vector (m/s^2) in Body Frame.

@@ -122,13 +109,19 @@ 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;

-    return inv_get_6_axis_gyro_accel_timestamp(hal_out.gravity_sample_rate_us, timestamp);

+    if ((hal_out.accel_status & INV_NEW_DATA) || (hal_out.gyro_status & INV_NEW_DATA))

+        status = 1;

+    else

+        status = 0;

+    return status;

 }

 

 /* 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 41b72c6..85e88f3 100644
--- a/60xx/libsensors_iio/software/core/mllite/hal_outputs.h
+++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h
@@ -38,10 +38,6 @@ 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