Skip to content

Commit

Permalink
AP_InertialSensor: make the backend fast gyro rate configurable and i…
Browse files Browse the repository at this point in the history 
…ndependent from the accel rate

allow fast sampling of gyros on MPU6000 and MPU6500
  • Loading branch information
@andyp1per
andyp1per committed Jul 28, 2020
1 parent ccff006 commit a5281e0
Show file tree
Hide file tree
Showing 5 changed files with 108 additions and 64 deletions.
2 changes: 1 addition & 1 deletion libraries/AP_InertialSensor/AP_InertialSensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),

Expand Down
72 changes: 44 additions & 28 deletions libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -332,24 +332,24 @@ 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);
if (_fifo_buffer == nullptr) {
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;
Expand Down Expand Up @@ -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));
Expand All @@ -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));
Expand All @@ -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;
}
}

Expand Down Expand Up @@ -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;
Expand All @@ -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);
Expand Down
20 changes: 15 additions & 5 deletions libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,11 +138,20 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend
// 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;
Expand All @@ -157,7 +166,8 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend
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;
Expand Down
61 changes: 36 additions & 25 deletions libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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);
Expand All @@ -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;
Expand Down Expand Up @@ -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));
Expand All @@ -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;
}
}

Expand Down Expand Up @@ -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;
Expand All @@ -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);
Expand Down
17 changes: 12 additions & 5 deletions libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,11 +128,17 @@ class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend
// 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;
Expand All @@ -148,7 +154,8 @@ class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend
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;
Expand Down

0 comments on commit a5281e0

Please sign in to comment.