Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: make the backend fast gyro rate configurable

Browse Source 
raise gyro rate default on F7 and H7
clean up gyro rate docs and output startup banner
This commit is contained in:
Andy Piper 2020-05-21 19:29:28 +01:00 committed by Andrew Tridgell
parent e35458cedf
commit faf9bbbf3a
8 changed files with 108 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -40,6 +40,8 @@
extern const AP_HAL::HAL& hal;
#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
#define DEFAULT_GYRO_FILTER 20
#define DEFAULT_ACCEL_FILTER 20
@ -54,6 +56,12 @@ extern const AP_HAL::HAL& hal;
#define DEFAULT_STILL_THRESH 0.1f
#endif
#if defined(STM32H7) || defined(STM32F7)
#define MPU_FIFO_FASTSAMPLE_DEFAULT 1
#else
#define MPU_FIFO_FASTSAMPLE_DEFAULT 0
#endif
#define SAMPLE_UNIT 1
#define GYRO_INIT_MAX_DIFF_DPS 0.1f
@ -493,6 +501,14 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// @Path: ../Filter/HarmonicNotchFilter.cpp
AP_SUBGROUPINFO(_harmonic_notch_filter, "HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams),
// @Param: GYRO_RATE
// @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
// @RebootRequired: True
AP_GROUPINFO("GYRO_RATE", 42, AP_InertialSensor, _fast_sampling_rate, MPU_FIFO_FASTSAMPLE_DEFAULT),
/*
NOTE: parameter indexes have gaps above. When adding new
parameters check for conflicts carefully
@ -681,11 +697,11 @@ bool AP_InertialSensor::set_gyro_window_size(uint16_t size) {
}
void
AP_InertialSensor::init(uint16_t sample_rate)
AP_InertialSensor::init(uint16_t loop_rate)
{
// remember the sample rate
_sample_rate = sample_rate;
_loop_delta_t = 1.0f / sample_rate;
_loop_rate = loop_rate;
_loop_delta_t = 1.0f / loop_rate;
// we don't allow deltat values greater than 10x the normal loop
// time to be exposed outside of INS. Large deltat values can
@ -709,7 +725,7 @@ AP_InertialSensor::init(uint16_t sample_rate)
init_gyro();
}
_sample_period_usec = 1000*1000UL / _sample_rate;
_sample_period_usec = 1000*1000UL / _loop_rate;
// establish the baseline time between samples
_delta_time = 0;
@ -979,6 +995,16 @@ AP_InertialSensor::init_gyro()
_save_gyro_calibration();
}
// output GCS startup messages
bool AP_InertialSensor::get_output_banner(uint8_t backend_id, char* banner, uint8_t banner_len)
{
if (backend_id >= INS_MAX_BACKENDS || _backends[backend_id] == nullptr) {
return false;
}
return _backends[backend_id]->get_output_banner(banner, banner_len);
}
// accelerometer clipping reporting
uint32_t AP_InertialSensor::get_accel_clip_count(uint8_t instance) const
{
@ -1055,7 +1081,7 @@ bool AP_InertialSensor::calibrate_trim(float &trim_roll, float &trim_pitch)
_calibrating = true;
const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_sample_rate()+0.5f);
const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_loop_rate_hz()+0.5f);
// wait 100ms for ins filter to rise
for (uint8_t k=0; k<100/update_dt_milliseconds; k++) {

14
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -97,6 +97,9 @@ public:
///
void init_gyro(void);
// get startup messages to output to the GCS
bool get_output_banner(uint8_t instance_id, char* banner, uint8_t banner_len);
/// Fetch the current gyro values
///
/// @returns vector of rotational rates in radians/sec
@ -206,8 +209,8 @@ public:
_custom_rotation = custom_rotation;
}
// return the selected sample rate
uint16_t get_sample_rate(void) const { return _sample_rate; }
// return the selected loop rate at which samples are made avilable
uint16_t get_loop_rate_hz(void) const { return _loop_rate; }
// return the main loop delta_t in seconds
float get_loop_delta_t(void) const { return _loop_delta_t; }
@ -433,8 +436,8 @@ private:
uint8_t _accel_count;
uint8_t _backend_count;
// the selected sample rate
uint16_t _sample_rate;
// the selected loop rate at which samples are made available
uint16_t _loop_rate;
float _loop_delta_t;
float _loop_delta_t_max;
@ -541,6 +544,9 @@ private:
// control enable of fast sampling
AP_Int8 _fast_sampling_mask;
// control enable of fast sampling
AP_Int8 _fast_sampling_rate;
// control enable of detected sensors
AP_Int8 _enable_mask;

6
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -479,11 +479,11 @@ void AP_InertialSensor_Backend::_inc_gyro_error_count(uint8_t instance)
_imu._gyro_error_count[instance]++;
}
// return the requested sample rate in Hz
uint16_t AP_InertialSensor_Backend::get_sample_rate_hz(void) const
// return the requested loop rate at which samples will be made available in Hz
uint16_t AP_InertialSensor_Backend::get_loop_rate_hz(void) const
{
// enum can be directly cast to Hz
return (uint16_t)_imu._sample_rate;
return (uint16_t)_imu._loop_rate;
}
/*

10
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -76,6 +76,9 @@ public:
// notify of a fifo reset
void notify_fifo_reset(void);
// get a startup banner to output to the GCS
virtual bool get_output_banner(char* banner, uint8_t banner_len) { return false; }
/*
device driver IDs. These are used to fill in the devtype field
of the device ID, which shows up as INS*ID* parameters to
@ -234,7 +237,7 @@ protected:
uint16_t _gyro_filter_cutoff(void) const { return _imu._gyro_filter_cutoff; }
// return the requested sample rate in Hz
uint16_t get_sample_rate_hz(void) const;
uint16_t get_loop_rate_hz(void) const;
// return the notch filter center in Hz for the sample rate
float _gyro_notch_center_freq_hz(void) const { return _imu._notch_filter.center_freq_hz(); }
@ -301,6 +304,11 @@ protected:
return (_imu._fast_sampling_mask & (1U<<instance)) != 0;
}
// if fast sampling is enabled, the rate to use in kHz
uint8_t get_fast_sampling_rate() {
return (1 << uint8_t(_imu._fast_sampling_rate));
}
// called by subclass when data is received from the sensor, thus
// at the 'sensor rate'
void _notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel);

41
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -265,11 +265,6 @@ void AP_InertialSensor_Invensense::start()
// readings to fit them into an int16_t:
_set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);
if (_fast_sampling) {
hal.console->printf("MPU[%u]: enabled fast sampling rate %uHz/%uHz\n",
_accel_instance, _backend_rate_hz*_fifo_downsample_rate, _backend_rate_hz);
}
// set sample rate to 1000Hz and apply a software filter
// In this configuration, the gyro sample rate is 8kHz
_register_write(MPUREG_SMPLRT_DIV, 0, true);
@ -350,6 +345,15 @@ void AP_InertialSensor_Invensense::start()
_dev->register_periodic_callback(1000000UL / _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);
return true;
}
return false;
}
/*
publish any pending data
@ -509,13 +513,13 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
_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);
_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;
@ -684,15 +688,20 @@ void AP_InertialSensor_Invensense::_set_filter_register(void)
if (enable_fast_sampling(_accel_instance)) {
_fast_sampling = (_mpu_type >= Invensense_MPU9250 && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
if (_fast_sampling) {
if (get_sample_rate_hz() <= 1000) {
_fifo_downsample_rate = 8;
} else if (get_sample_rate_hz() <= 2000) {
_fifo_downsample_rate = 4;
} else {
_fifo_downsample_rate = 2;
// constrain the gyro rate to be at least the loop rate
uint8_t loop_limit = 1;
if (get_loop_rate_hz() > 1000) {
loop_limit = 2;
}
// calculate rate we will be giving samples to the backend
_backend_rate_hz *= (8 / _fifo_downsample_rate);
if (get_loop_rate_hz() > 2000) {
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);
// calculate rate we will be giving gyro samples to the backend
_fifo_downsample_rate = 8 / fast_sampling_rate;
_backend_rate_hz *= fast_sampling_rate;
// for logging purposes set the oversamping rate
_set_accel_oversampling(_accel_instance, _fifo_downsample_rate/2);

3
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h
View File

@ -56,6 +56,9 @@ public:
void start() override;
// get a startup banner to output to the GCS
bool get_output_banner(char* banner, uint8_t banner_len) override;
enum Invensense_Type {
Invensense_MPU6000=0,
Invensense_MPU6500,

39
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp
View File

@ -207,11 +207,6 @@ void AP_InertialSensor_Invensensev2::start()
// readings to fit them into an int16_t:
_set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);
if (_fast_sampling) {
hal.console->printf("INV2[%u]: enabled fast sampling rate %uHz/%uHz\n",
_accel_instance, _backend_rate_hz*_fifo_downsample_rate, _backend_rate_hz);
}
// set sample rate to 1.125KHz
_register_write(INV2REG_GYRO_SMPLRT_DIV, 0, true);
hal.scheduler->delay(1);
@ -243,6 +238,15 @@ void AP_InertialSensor_Invensensev2::start()
_dev->register_periodic_callback(1265625UL / _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);
return true;
}
return false;
}
/*
publish any pending data
@ -402,10 +406,10 @@ bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *s
_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;
@ -580,15 +584,20 @@ void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void)
if (enable_fast_sampling(_accel_instance)) {
_fast_sampling = _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
if (_fast_sampling) {
if (get_sample_rate_hz() <= 1125) {
_fifo_downsample_rate = 8;
} else if (get_sample_rate_hz() <= 2250) {
_fifo_downsample_rate = 4;
} else {
_fifo_downsample_rate = 2;
// constrain the gyro rate to be at least the loop rate
uint8_t loop_limit = 1;
if (get_loop_rate_hz() > 1125) {
loop_limit = 2;
}
// calculate rate we will be giving samples to the backend
_backend_rate_hz *= (8 / _fifo_downsample_rate);
if (get_loop_rate_hz() > 2250) {
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, 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;
// for logging purposes set the oversamping rate
_set_accel_oversampling(_accel_instance, _fifo_downsample_rate/2);

3
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h
View File

@ -52,6 +52,9 @@ public:
void start() override;
// get a startup banner to output to the GCS
bool get_output_banner(char* banner, uint8_t banner_len) override;
enum Invensensev2_Type {
Invensensev2_ICM20948 = 0,
Invensensev2_ICM20648,