From db25b6e96667f6cab9d4de5045e5feaa3234b403 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 1 May 2017 13:01:43 +1000 Subject: [PATCH] AP_InertialSensor: estimate and log sensor rates for all IMUs this adds IMU.GHz and IMU.AHz log fields so we can see the actual observed sensor rates of each IMU --- .../AP_InertialSensor/AP_InertialSensor.cpp | 3 +++ .../AP_InertialSensor/AP_InertialSensor.h | 8 +++++++ .../AP_InertialSensor_Backend.cpp | 24 ++++++++++++++----- .../AP_InertialSensor_Backend.h | 6 +++++ .../AP_InertialSensor_Invensense.cpp | 4 ++++ 5 files changed, 39 insertions(+), 6 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 70151d0f36..eb65b21479 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -512,6 +512,7 @@ uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz, } _gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz; + _gyro_over_sampling[_gyro_count] = 1; bool saved = _gyro_id[_gyro_count].load(); @@ -544,6 +545,8 @@ uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz, } _accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz; + _accel_over_sampling[_accel_count] = 1; + bool saved = _accel_id[_accel_count].load(); if (!saved) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 61c952dfe6..915215ca93 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -133,6 +133,10 @@ public: bool accel_calibrated_ok_all() const; bool use_accel(uint8_t instance) const; + // get observed sensor rates, including any internal sampling multiplier + uint16_t get_gyro_rate_hz(uint8_t instance) const { return uint16_t(_gyro_raw_sample_rates[instance] * _gyro_over_sampling[instance]); } + uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); } + // get accel offsets in m/s/s const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; } const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); } @@ -345,6 +349,10 @@ private: float _accel_raw_sample_rates[INS_MAX_INSTANCES]; float _gyro_raw_sample_rates[INS_MAX_INSTANCES]; + // how many sensors samples per notify to the backend + uint8_t _accel_over_sampling[INS_MAX_INSTANCES]; + uint8_t _gyro_over_sampling[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]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 2e84684f03..7d0e5082d0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -33,6 +33,18 @@ void AP_InertialSensor_Backend::notify_gyro_fifo_reset(uint8_t instance) _imu._sample_gyro_start_us[instance] = 0; } +// set the amount of oversamping a accel is doing +void AP_InertialSensor_Backend::_set_accel_oversampling(uint8_t instance, uint8_t n) +{ + _imu._accel_over_sampling[instance] = n; +} + +// set the amount of oversamping a gyro is doing +void AP_InertialSensor_Backend::_set_gyro_oversampling(uint8_t instance, uint8_t n) +{ + _imu._gyro_over_sampling[instance] = n; +} + /* update the sensor rate for FIFO sensors @@ -116,6 +128,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, { float dt; + _update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance], + _imu._gyro_raw_sample_rates[instance]); + /* we have two classes of sensors. FIFO based sensors produce data at a very predictable overall rate, but the data comes in @@ -127,9 +142,6 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, 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]); - // don't accept below 100Hz if (_imu._gyro_raw_sample_rates[instance] < 100) { return; @@ -234,6 +246,9 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance, { float dt; + _update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance], + _imu._accel_raw_sample_rates[instance]); + /* we have two classes of sensors. FIFO based sensors produce data at a very predictable overall rate, but the data comes in @@ -245,9 +260,6 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance, 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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 84af487f4f..6f8cfa6956 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -125,6 +125,12 @@ protected: // 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); + // set the amount of oversamping a accel is doing + void _set_accel_oversampling(uint8_t instance, uint8_t n); + + // set the amount of oversamping a gyro is doing + void _set_gyro_oversampling(uint8_t instance, uint8_t n); + // update the sensor rate for FIFO sensors void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index cb1f3461fd..096b514d15 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -810,6 +810,10 @@ void AP_InertialSensor_Invensense::_set_filter_register(void) _fast_sampling = (_mpu_type != Invensense_MPU6000 && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI); if (_fast_sampling) { hal.console->printf("MPU[%u]: enabled fast sampling\n", _accel_instance); + + // for logging purposes set the oversamping rate + _set_accel_oversampling(_accel_instance, MPU_FIFO_DOWNSAMPLE_COUNT/2); + _set_gyro_oversampling(_accel_instance, MPU_FIFO_DOWNSAMPLE_COUNT); } }