aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: use multiply for fifo scale factors

Browse Source 
this fixes issue from #8118 too
This commit is contained in:
Andrew Tridgell 2018-04-10 12:16:45 +10:00
parent 8027883734
commit f788cde5c7
2 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -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);

3
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View File

@ -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;