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: Ie06912843e82811a6728cf0a67ce1cf801efa998

1 files changed, 16 insertions, 9 deletions

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, &timestamp1);

     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.