Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: make the backend fast gyro rate configurable and independent from the accel rate 

allow fast sampling of gyros on MPU6000 and MPU6500
This commit is contained in:
Andy Piper 2020-05-21 19:29:28 +01:00 committed by Andrew Tridgell
parent 5cebf91c3f
commit d4ba821297
5 changed files with 108 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -505,7 +505,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// @DisplayName: Gyro rate for IMUs with Fast Sampling enabled
// @Description: Gyro rate for IMUs with fast sampling enabled. The gyro rate is the sample rate at which the IMU filters operate and needs to be at least double the maximum filter frequency. If the sensor does not support the selected rate the next highest supported rate will be used. For IMUs which do not support fast sampling this setting is ignored and the default gyro rate of 1Khz is used.
// @User: Advanced
// @Values: 0:1 kHz,1:2 kHz,3:4 kHz
// @Values: 0:1kHz,1:2kHz,2:4kHz,3:8kHz
// @RebootRequired: True
AP_GROUPINFO("GYRO_RATE", 42, AP_InertialSensor, _fast_sampling_rate, MPU_FIFO_FASTSAMPLE_DEFAULT),

72
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -258,8 +258,8 @@ void AP_InertialSensor_Invensense::start()
_set_filter_register();
// update backend sample rate
_set_accel_raw_sample_rate(_accel_instance, _backend_rate_hz);
_set_gyro_raw_sample_rate(_gyro_instance, _backend_rate_hz);
_set_accel_raw_sample_rate(_accel_instance, _accel_backend_rate_hz);
_set_gyro_raw_sample_rate(_gyro_instance, _gyro_backend_rate_hz);
// indicate what multiplier is appropriate for the sensors'
// readings to fit them into an int16_t:
@ -332,8 +332,8 @@ void AP_InertialSensor_Invensense::start()
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;
_fifo_accel_scale = _accel_scale / _accel_fifo_downsample_rate;
_fifo_gyro_scale = _gyro_scale / _gyro_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);
@ -341,15 +341,15 @@ void AP_InertialSensor_Invensense::start()
AP_HAL::panic("Invensense: Unable to allocate FIFO buffer");
}
// start the timer process to read samples
_dev->register_periodic_callback(1000000UL / _backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, void));
// start the timer process to read samples, using the fastest rate avilable
_dev->register_periodic_callback(1000000UL / _gyro_backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, void));
}
// get a startup banner to output to the GCS
bool AP_InertialSensor_Invensense::get_output_banner(char* banner, uint8_t banner_len) {
if (_fast_sampling) {
snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz",
_accel_instance, _backend_rate_hz*_fifo_downsample_rate / 1000.0, _backend_rate_hz/ 1000.0);
_gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate / 1000.0, _gyro_backend_rate_hz / 1000.0);
return true;
}
return false;
@ -484,8 +484,8 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
}
tsum += t2;
if ((_accum.count & 1) == 0) {
// accel data is at 4kHz
if (_accum.gyro_count % _gyro_to_accel_sample_ratio == 0) {
// accel data is at 4kHz or 1kHz
Vector3f a(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
@ -495,10 +495,25 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
clipped = true;
}
_accum.accel += _accum.accel_filter.apply(a);
Vector3f a2 = a * _accel_scale;
_notify_new_accel_sensor_rate_sample(_accel_instance, a2);
_accum.accel_count++;
if (_accum.accel_count % _accel_fifo_downsample_rate == 0) {
_accum.accel *= _fifo_accel_scale;
_rotate_and_correct_accel(_accel_instance, _accum.accel);
_notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
_accum.accel.zero();
_accum.accel_count = 0;
// we assume that the gyro rate is always >= and a multiple of the accel rate
_accum.gyro_count = 0;
}
}
_accum.gyro_count++;
Vector3f g(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
@ -507,22 +522,12 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
_notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);
_accum.gyro += _accum.gyro_filter.apply(g);
_accum.count++;
if (_accum.count == _fifo_downsample_rate) {
_accum.accel *= _fifo_accel_scale;
if (_accum.gyro_count % _gyro_fifo_downsample_rate == 0) {
_accum.gyro *= _fifo_gyro_scale;
_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, 0, false);
_notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
_accum.accel.zero();
_accum.gyro.zero();
_accum.count = 0;
}
}
@ -682,11 +687,12 @@ void AP_InertialSensor_Invensense::_set_filter_register(void)
#endif
// assume 1kHz sampling to start
_fifo_downsample_rate = 1;
_backend_rate_hz = 1000;
_gyro_fifo_downsample_rate = _accel_fifo_downsample_rate = 1;
_gyro_to_accel_sample_ratio = 2;
_gyro_backend_rate_hz = _accel_backend_rate_hz = 1000;
if (enable_fast_sampling(_accel_instance)) {
_fast_sampling = (_mpu_type >= Invensense_MPU9250 && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
_fast_sampling = _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
if (_fast_sampling) {
// constrain the gyro rate to be at least the loop rate
uint8_t loop_limit = 1;
@ -697,15 +703,25 @@ void AP_InertialSensor_Invensense::_set_filter_register(void)
loop_limit = 4;
}
// constrain the gyro rate to be a 2^N multiple
uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 4);
uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8);
// calculate rate we will be giving gyro samples to the backend
_fifo_downsample_rate = 8 / fast_sampling_rate;
_backend_rate_hz *= fast_sampling_rate;
_gyro_fifo_downsample_rate = 8 / fast_sampling_rate;
_gyro_backend_rate_hz *= fast_sampling_rate;
// calculate rate we will be giving accel samples to the backend
if (_mpu_type >= Invensense_MPU9250) {
_accel_fifo_downsample_rate = MAX(4 / fast_sampling_rate, 1);
_accel_backend_rate_hz *= MIN(fast_sampling_rate, 4);
} else {
_gyro_to_accel_sample_ratio = 8;
_accel_fifo_downsample_rate = 1;
_accum.accel_filter.set_cutoff_frequency(1000, 188);
}
// for logging purposes set the oversamping rate
_set_accel_oversampling(_accel_instance, _fifo_downsample_rate/2);
_set_gyro_oversampling(_gyro_instance, _fifo_downsample_rate);
_set_accel_oversampling(_accel_instance, _accel_fifo_downsample_rate);
_set_gyro_oversampling(_gyro_instance, _gyro_fifo_downsample_rate);
_set_accel_sensor_rate_sampling_enabled(_accel_instance, true);
_set_gyro_sensor_rate_sampling_enabled(_gyro_instance, true);

20
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View File

@ -138,11 +138,20 @@ private:
// are we doing more than 1kHz sampling?
bool _fast_sampling;
// what downsampling rate are we using from the FIFO?
uint8_t _fifo_downsample_rate;
// what downsampling rate are we using from the FIFO for gyros?
uint8_t _gyro_fifo_downsample_rate;
// what rate are we generating samples into the backend?
uint16_t _backend_rate_hz;
// what downsampling rate are we using from the FIFO for accels?
uint8_t _accel_fifo_downsample_rate;
// ratio of raw gyro to accel sample rate
uint8_t _gyro_to_accel_sample_ratio;
// what rate are we generating samples into the backend for gyros?
uint16_t _gyro_backend_rate_hz;
// what rate are we generating samples into the backend for accels?
uint16_t _accel_backend_rate_hz;
// Last status from register user control
uint8_t _last_stat_user_ctrl;
@ -157,7 +166,8 @@ private:
struct {
Vector3f accel;
Vector3f gyro;
uint8_t count;
uint8_t accel_count;
uint8_t gyro_count;
LowPassFilterVector3f accel_filter{4000, 188};
LowPassFilterVector3f gyro_filter{8000, 188};
} _accum;

61
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
View File

@ -200,8 +200,8 @@ void AP_InertialSensor_Invensensev2::start()
_register_write(INV2REG_FSYNC_CONFIG, FSYNC_CONFIG_EXT_SYNC_AZ, true);
#endif
// update backend sample rate
_set_accel_raw_sample_rate(_accel_instance, _backend_rate_hz);
_set_gyro_raw_sample_rate(_gyro_instance, _backend_rate_hz);
_set_accel_raw_sample_rate(_accel_instance, _accel_backend_rate_hz);
_set_gyro_raw_sample_rate(_gyro_instance, _gyro_backend_rate_hz);
// indicate what multiplier is appropriate for the sensors'
// readings to fit them into an int16_t:
@ -225,8 +225,8 @@ void AP_InertialSensor_Invensensev2::start()
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;
_fifo_accel_scale = _accel_scale / _accel_fifo_downsample_rate;
_fifo_gyro_scale = GYRO_SCALE / _gyro_fifo_downsample_rate;
// allocate fifo buffer
_fifo_buffer = (uint8_t *)hal.util->malloc_type(INV2_FIFO_BUFFER_LEN * INV2_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
@ -235,14 +235,14 @@ void AP_InertialSensor_Invensensev2::start()
}
// start the timer process to read samples
_dev->register_periodic_callback(1265625UL / _backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev2::_poll_data, void));
_dev->register_periodic_callback(1265625UL / _gyro_backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev2::_poll_data, void));
}
// get a startup banner to output to the GCS
bool AP_InertialSensor_Invensensev2::get_output_banner(char* banner, uint8_t banner_len) {
if (_fast_sampling) {
snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz/%.1fkHz",
_accel_instance, _backend_rate_hz*_fifo_downsample_rate / 1000.0, _backend_rate_hz/ 1000.0);
_gyro_instance, _gyro_backend_rate_hz * _gyro_fifo_downsample_rate / 1000.0, _gyro_backend_rate_hz / 1000.0);
return true;
}
return false;
@ -376,21 +376,36 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s
break;
}
tsum += t2;
// accel data is at 4kHz
if ((_accum.count & 1) == 0) {
if (_accum.gyro_count % 2 == 0) {
// accel data is at 4kHz or 1kHz
Vector3f a(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
int16_val(data, 0),
-int16_val(data, 2));
if (fabsf(a.x) > unscaled_clip_limit ||
fabsf(a.y) > unscaled_clip_limit ||
fabsf(a.z) > unscaled_clip_limit) {
clipped = true;
}
_accum.accel += _accum.accel_filter.apply(a);
Vector3f a2 = a * _accel_scale;
_notify_new_accel_sensor_rate_sample(_accel_instance, a2);
_accum.accel_count++;
if (_accum.accel_count % _accel_fifo_downsample_rate == 0) {
_accum.accel *= _fifo_accel_scale;
_rotate_and_correct_accel(_accel_instance, _accum.accel);
_notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
_accum.accel.zero();
_accum.accel_count = 0;
// we assume that the gyro rate is always >= and a multiple of the accel rate
_accum.gyro_count = 0;
}
}
_accum.gyro_count++;
Vector3f g(int16_val(data, 4),
int16_val(data, 3),
-int16_val(data, 5));
@ -399,20 +414,12 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s
_notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);
_accum.gyro += _accum.gyro_filter.apply(g);
_accum.count++;
if (_accum.count == _fifo_downsample_rate) {
_accum.accel *= _fifo_accel_scale;
if (_accum.gyro_count % _gyro_fifo_downsample_rate == 0) {
_accum.gyro *= _fifo_gyro_scale;
_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, 0, false);
_notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
_accum.accel.zero();
_accum.gyro.zero();
_accum.count = 0;
}
}
@ -578,8 +585,8 @@ void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void)
uint8_t accel_config = (_inv2_type == Invensensev2_ICM20649)?BITS_ACCEL_FS_30G_20649:BITS_ACCEL_FS_16G;
// assume 1.125kHz sampling to start
_fifo_downsample_rate = 1;
_backend_rate_hz = 1125;
_gyro_fifo_downsample_rate = _accel_fifo_downsample_rate = 1;
_gyro_backend_rate_hz = _accel_backend_rate_hz = 1125;
if (enable_fast_sampling(_accel_instance)) {
_fast_sampling = _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
@ -596,12 +603,16 @@ void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void)
uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8);
// calculate rate we will be giving gyro samples to the backend
_fifo_downsample_rate = 8 / fast_sampling_rate;
_backend_rate_hz *= fast_sampling_rate;
_gyro_fifo_downsample_rate = 8 / fast_sampling_rate;
_gyro_backend_rate_hz *= fast_sampling_rate;
// calculate rate we will be giving accel samples to the backend
_accel_fifo_downsample_rate = MAX(4 / fast_sampling_rate, 1);
_accel_backend_rate_hz *= MIN(fast_sampling_rate, 4);
// for logging purposes set the oversamping rate
_set_accel_oversampling(_accel_instance, _fifo_downsample_rate/2);
_set_gyro_oversampling(_gyro_instance, _fifo_downsample_rate);
_set_accel_oversampling(_accel_instance, _accel_fifo_downsample_rate);
_set_gyro_oversampling(_gyro_instance, _gyro_fifo_downsample_rate);
_set_accel_sensor_rate_sampling_enabled(_accel_instance, true);
_set_gyro_sensor_rate_sampling_enabled(_gyro_instance, true);

17
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h
View File

@ -128,11 +128,17 @@ private:
// are we doing more than 1kHz sampling?
bool _fast_sampling;
// what downsampling rate are we using from the FIFO?
uint8_t _fifo_downsample_rate;
// what downsampling rate are we using from the FIFO for gyros?
uint8_t _gyro_fifo_downsample_rate;
// what rate are we generating samples into the backend?
uint16_t _backend_rate_hz;
// what downsampling rate are we using from the FIFO for accels?
uint8_t _accel_fifo_downsample_rate;
// what rate are we generating samples into the backend for gyros?
uint16_t _gyro_backend_rate_hz;
// what rate are we generating samples into the backend for accels?
uint16_t _accel_backend_rate_hz;
// Last status from register user control
uint8_t _last_stat_user_ctrl;
@ -148,7 +154,8 @@ private:
struct {
Vector3f accel;
Vector3f gyro;
uint8_t count;
uint8_t accel_count;
uint8_t gyro_count;
LowPassFilterVector3f accel_filter{4500, 188};
LowPassFilterVector3f gyro_filter{9000, 188};
} _accum;