From f788cde5c78744a3ded345677590b1fae5159d15 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Apr 2018 12:16:45 +1000 Subject: [PATCH] AP_InertialSensor: use multiply for fifo scale factors this fixes issue from #8118 too --- .../AP_InertialSensor_Invensense.cpp | 12 +++++++----- .../AP_InertialSensor/AP_InertialSensor_Invensense.h | 3 +++ 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index 47451bf241..5d816c4d6c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -317,6 +317,10 @@ void AP_InertialSensor_Invensense::start() set_gyro_orientation(_gyro_instance, _rotation); set_accel_orientation(_accel_instance, _rotation); + // setup scale factors for fifo data after downsampling + _fifo_accel_scale = _accel_scale / (MAX(_fifo_downsample_rate,2)/2); + _fifo_gyro_scale = GYRO_SCALE / _fifo_downsample_rate; + // allocate fifo buffer _fifo_buffer = (uint8_t *)hal.util->malloc_type(MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); if (_fifo_buffer == nullptr) { @@ -474,11 +478,9 @@ bool AP_InertialSensor_Invensense::_accumulate_fast_sampling(uint8_t *samples, u _accum.count++; if (_accum.count == _fifo_downsample_rate) { - float ascale = _accel_scale / (_fifo_downsample_rate/2); - _accum.accel *= ascale; - float gscale = GYRO_SCALE / _fifo_downsample_rate; - _accum.gyro *= gscale; + _accum.accel *= _fifo_accel_scale; + _accum.gyro *= _fifo_gyro_scale; _rotate_and_correct_accel(_accel_instance, _accum.accel); _rotate_and_correct_gyro(_gyro_instance, _accum.gyro); @@ -659,7 +661,7 @@ void AP_InertialSensor_Invensense::_set_filter_register(void) } // calculate rate we will be giving samples to the backend _backend_rate_hz *= (8 / _fifo_downsample_rate); - + // for logging purposes set the oversamping rate _set_accel_oversampling(_accel_instance, _fifo_downsample_rate/2); _set_gyro_oversampling(_gyro_instance, _fifo_downsample_rate); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index e6dfb0d5cc..e6d541eea6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -110,6 +110,9 @@ private: float _temp_filtered; float _accel_scale; + + float _fifo_accel_scale; + float _fifo_gyro_scale; LowPassFilter2pFloat _temp_filter; enum Rotation _rotation;