From 49cc0965c04795eda51cfa87b0022177a84615de Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 1 May 2017 10:53:41 +1000 Subject: [PATCH] AP_InertialSensor: separate handling of FIFO and non-FIFO sensors FIFO sensors produce data at a well known rate, but samples come in bunches, so we can't use the system clock to calculate deltaT. non-FIFO sensors produce data when we sample them, but that rate is less regular due to timing jitter. For FIFO sensors this changes makes us use a learned sample rate, which allows for different clock speeds on sensor and system board. For non-FIFO sensors we use the system clock to measure deltaT the overall effect is a fix for sensors that produce samples at other than the claimed datasheet rate. --- .../AP_InertialSensor/AP_InertialSensor.h | 15 ++- .../AP_InertialSensor_Backend.cpp | 109 +++++++++++++++--- .../AP_InertialSensor_Backend.h | 24 +++- .../AP_InertialSensor_Invensense.cpp | 7 +- .../AP_InertialSensor_LSM9DS0.cpp | 4 +- 5 files changed, 132 insertions(+), 27 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 446bf771ad..61c952dfe6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -342,9 +342,20 @@ private: float _accel_max_abs_offsets[INS_MAX_INSTANCES]; // accelerometer and gyro raw sample rate in units of Hz - uint16_t _accel_raw_sample_rates[INS_MAX_INSTANCES]; - uint16_t _gyro_raw_sample_rates[INS_MAX_INSTANCES]; + float _accel_raw_sample_rates[INS_MAX_INSTANCES]; + float _gyro_raw_sample_rates[INS_MAX_INSTANCES]; + // last sample time in microseconds. Use for deltaT calculations + // on non-FIFO sensors + uint64_t _accel_last_sample_us[INS_MAX_INSTANCES]; + uint64_t _gyro_last_sample_us[INS_MAX_INSTANCES]; + + // sample times for checking real sensor rate for FIFO sensors + uint16_t _sample_accel_count[INS_MAX_INSTANCES]; + uint32_t _sample_accel_start_us[INS_MAX_INSTANCES]; + uint16_t _sample_gyro_count[INS_MAX_INSTANCES]; + uint32_t _sample_gyro_start_us[INS_MAX_INSTANCES]; + // temperatures for an instance if available float _temperature[INS_MAX_INSTANCES]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 967486b985..2e84684f03 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -3,6 +3,9 @@ #include "AP_InertialSensor_Backend.h" #include #include +#include + +#define SENSOR_RATE_DEBUG 0 const extern AP_HAL::HAL& hal; @@ -12,6 +15,52 @@ AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) : _sem = hal.util->new_semaphore(); } +/* + notify of a FIFO reset so we don't use bad data to update observed sensor rate + */ +void AP_InertialSensor_Backend::notify_accel_fifo_reset(uint8_t instance) +{ + _imu._sample_accel_count[instance] = 0; + _imu._sample_accel_start_us[instance] = 0; +} + +/* + notify of a FIFO reset so we don't use bad data to update observed sensor rate + */ +void AP_InertialSensor_Backend::notify_gyro_fifo_reset(uint8_t instance) +{ + _imu._sample_gyro_count[instance] = 0; + _imu._sample_gyro_start_us[instance] = 0; +} + +/* + update the sensor rate for FIFO sensors + + FIFO sensors produce samples at a fixed rate, but the clock in the + sensor may vary slightly from the system clock. This slowly adjusts + the rate to the observed rate +*/ +void AP_InertialSensor_Backend::_update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) +{ + uint32_t now = AP_HAL::micros(); + if (start_us == 0) { + count = 0; + start_us = now; + } else { + count++; + if (now - start_us > 1000000UL) { + float observed_rate_hz = count * 1.0e6 / (now - start_us); +#if SENSOR_RATE_DEBUG + printf("RATE: %.1f should be %.1f\n", observed_rate_hz, rate_hz); +#endif + observed_rate_hz = constrain_float(observed_rate_hz, rate_hz*0.95, rate_hz*1.05); + rate_hz = 0.90 * rate_hz + 0.1 * observed_rate_hz; + count = 0; + start_us = now; + } + } +} + void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vector3f &accel) { /* @@ -55,10 +104,6 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f & _imu._gyro[instance] = gyro; _imu._gyro_healthy[instance] = true; - if (_imu._gyro_raw_sample_rates[instance] <= 0) { - return; - } - // publish delta angle _imu._delta_angle[instance] = _imu._delta_angle_acc[instance]; _imu._delta_angle_dt[instance] = _imu._delta_angle_acc_dt[instance]; @@ -71,11 +116,28 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, { float dt; - if (_imu._gyro_raw_sample_rates[instance] <= 0) { - return; - } + /* + we have two classes of sensors. FIFO based sensors produce data + at a very predictable overall rate, but the data comes in + bunches, so we use the provided sample rate for deltaT. Non-FIFO + sensors don't bunch up samples, but also tend to vary in actual + rate, so we use the provided sample_us to get the deltaT. The + difference between the two is whether sample_us is provided. + */ + if (sample_us != 0 && _imu._gyro_last_sample_us[instance] != 0) { + dt = (sample_us - _imu._gyro_last_sample_us[instance]) * 1.0e-6; + } else { + _update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance], + _imu._gyro_raw_sample_rates[instance]); - dt = 1.0f / _imu._gyro_raw_sample_rates[instance]; + // don't accept below 100Hz + if (_imu._gyro_raw_sample_rates[instance] < 100) { + return; + } + + dt = 1.0f / _imu._gyro_raw_sample_rates[instance]; + } + _imu._gyro_last_sample_us[instance] = sample_us; // call gyro_sample hook if any AP_Module::call_hook_gyro_sample(instance, dt, gyro); @@ -140,10 +202,6 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f _imu._accel[instance] = accel; _imu._accel_healthy[instance] = true; - if (_imu._accel_raw_sample_rates[instance] <= 0) { - return; - } - // publish delta velocity _imu._delta_velocity[instance] = _imu._delta_velocity_acc[instance]; _imu._delta_velocity_dt[instance] = _imu._delta_velocity_acc_dt[instance]; @@ -176,13 +234,30 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance, { float dt; - if (_imu._accel_raw_sample_rates[instance] <= 0) { - return; + /* + we have two classes of sensors. FIFO based sensors produce data + at a very predictable overall rate, but the data comes in + bunches, so we use the provided sample rate for deltaT. Non-FIFO + sensors don't bunch up samples, but also tend to vary in actual + rate, so we use the provided sample_us to get the deltaT. The + difference between the two is whether sample_us is provided. + */ + if (sample_us != 0 && _imu._accel_last_sample_us[instance] != 0) { + dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6; + } else { + _update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance], + _imu._accel_raw_sample_rates[instance]); + + // don't accept below 100Hz + if (_imu._accel_raw_sample_rates[instance] < 100) { + return; + } + + dt = 1.0f / _imu._accel_raw_sample_rates[instance]; } + _imu._accel_last_sample_us[instance] = sample_us; - dt = 1.0f / _imu._accel_raw_sample_rates[instance]; - - // call gyro_sample hook if any + // call accel_sample hook if any AP_Module::call_hook_accel_sample(instance, dt, accel, fsync_set); _imu.calc_vibration_and_clipping(instance, accel, dt); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 89ef63b61a..84af487f4f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -70,6 +70,9 @@ public: */ int16_t get_id() const { return _id; } + // notify of a fifo reset + void notify_fifo_reset(void); + /* device driver IDs. These are used to fill in the devtype field of the device ID, which shows up as INS*ID* parameters to @@ -103,10 +106,12 @@ protected: // rotate gyro vector, offset and publish void _publish_gyro(uint8_t instance, const Vector3f &gyro); - // this should be called every time a new gyro raw sample is available - - // be it published or not - // the sample is raw in the sense that it's not filtered yet, but it must - // be rotated and corrected (_rotate_and_correct_gyro) + // this should be called every time a new gyro raw sample is + // available - be it published or not the sample is raw in the + // sense that it's not filtered yet, but it must be rotated and + // corrected (_rotate_and_correct_gyro) + // The sample_us value must be provided for non-FIFO based + // sensors, and should be set to zero for FIFO based sensors void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0); // rotate accel vector, scale, offset and publish @@ -116,8 +121,13 @@ protected: // be it published or not // the sample is raw in the sense that it's not filtered yet, but it must // be rotated and corrected (_rotate_and_correct_accel) + // The sample_us value must be provided for non-FIFO based + // sensors, and should be set to zero for FIFO based sensors void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false); + // update the sensor rate for FIFO sensors + void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz); + // set accelerometer max absolute offset for calibration void _set_accel_max_abs_offset(uint8_t instance, float offset); @@ -191,6 +201,12 @@ protected: bool enable_fast_sampling(uint8_t instance) { return (_imu._fast_sampling_mask & (1U<delay_microseconds(1); _dev->set_speed(AP_HAL::Device::SPEED_HIGH); _last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN; + + notify_accel_fifo_reset(_accel_instance); + notify_gyro_fifo_reset(_gyro_instance); } bool AP_InertialSensor_Invensense::_has_auxiliary_bus() @@ -587,7 +590,7 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl _rotate_and_correct_accel(_accel_instance, accel); _rotate_and_correct_gyro(_gyro_instance, gyro); - _notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), fsync_set); + _notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set); _notify_new_gyro_raw_sample(_gyro_instance, gyro); _temp_filtered = _temp_filter.apply(temp); @@ -653,7 +656,7 @@ bool AP_InertialSensor_Invensense::_accumulate_fast_sampling(uint8_t *samples, u _rotate_and_correct_accel(_accel_instance, _accum.accel); _rotate_and_correct_gyro(_gyro_instance, _accum.gyro); - _notify_new_accel_raw_sample(_accel_instance, _accum.accel, AP_HAL::micros64(), false); + _notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false); _notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro); _accum.accel.zero(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index 2a81de228b..c74eeb0bda 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -744,7 +744,7 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_a() accel_data *= _accel_scale; _rotate_and_correct_accel(_accel_instance, accel_data); - _notify_new_accel_raw_sample(_accel_instance, accel_data); + _notify_new_accel_raw_sample(_accel_instance, accel_data, AP_HAL::micros64()); } /* @@ -765,7 +765,7 @@ void AP_InertialSensor_LSM9DS0::_read_data_transaction_g() gyro_data *= _gyro_scale; _rotate_and_correct_gyro(_gyro_instance, gyro_data); - _notify_new_gyro_raw_sample(_gyro_instance, gyro_data); + _notify_new_gyro_raw_sample(_gyro_instance, gyro_data, AP_HAL::micros64()); } bool AP_InertialSensor_LSM9DS0::update()