diff options
author | Mark Salyzyn <salyzyn@google.com> | 2015-02-25 20:52:54 +0000 |
---|---|---|
committer | Gerrit Code Review <noreply-gerritcodereview@google.com> | 2015-02-25 20:53:31 +0000 |
commit | eb77dbf00ed67a8e12ab376921300883f6975ce2 (patch) | |
tree | 6c57fb081a017af68726a07c21f7c23bc5017d5d /60xx/libsensors_iio/software/core | |
parent | e70d3877a2585b77e544d39c9fa03e8ff4a9e928 (diff) | |
parent | deb5d22e3769be80b6627c812dba9ea3b5a90f0a (diff) | |
download | invensense-eb77dbf00ed67a8e12ab376921300883f6975ce2.tar.gz |
Merge "Revert "manta: batching and timestamp issues""
Diffstat (limited to '60xx/libsensors_iio/software/core')
4 files changed, 17 insertions, 146 deletions
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, ×tamp1);
+ 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 |