Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: estimate and log sensor rates for all IMUs

Browse Source 
this adds IMU.GHz and IMU.AHz log fields so we can see the actual
observed sensor rates of each IMU
This commit is contained in:
Andrew Tridgell 2017-05-01 13:01:43 +10:00
parent 0a83281a29
commit db25b6e966
5 changed files with 39 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -512,6 +512,7 @@ uint8_t AP_InertialSensor::register_gyro(uint16_t raw_sample_rate_hz,
}
_gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
_gyro_over_sampling[_gyro_count] = 1;
bool saved = _gyro_id[_gyro_count].load();
@ -544,6 +545,8 @@ uint8_t AP_InertialSensor::register_accel(uint16_t raw_sample_rate_hz,
}
_accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
_accel_over_sampling[_accel_count] = 1;
bool saved = _accel_id[_accel_count].load();
if (!saved) {

8
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -133,6 +133,10 @@ public:
bool accel_calibrated_ok_all() const;
bool use_accel(uint8_t instance) const;
// get observed sensor rates, including any internal sampling multiplier
uint16_t get_gyro_rate_hz(uint8_t instance) const { return uint16_t(_gyro_raw_sample_rates[instance] * _gyro_over_sampling[instance]); }
uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); }
// get accel offsets in m/s/s
const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); }
@ -345,6 +349,10 @@ private:
float _accel_raw_sample_rates[INS_MAX_INSTANCES];
float _gyro_raw_sample_rates[INS_MAX_INSTANCES];
// how many sensors samples per notify to the backend
uint8_t _accel_over_sampling[INS_MAX_INSTANCES];
uint8_t _gyro_over_sampling[INS_MAX_INSTANCES];
// last sample time in microseconds. Use for deltaT calculations
// on non-FIFO sensors
uint64_t _accel_last_sample_us[INS_MAX_INSTANCES];

24
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -33,6 +33,18 @@ void AP_InertialSensor_Backend::notify_gyro_fifo_reset(uint8_t instance)
_imu._sample_gyro_start_us[instance] = 0;
}
// set the amount of oversamping a accel is doing
void AP_InertialSensor_Backend::_set_accel_oversampling(uint8_t instance, uint8_t n)
{
_imu._accel_over_sampling[instance] = n;
}
// set the amount of oversamping a gyro is doing
void AP_InertialSensor_Backend::_set_gyro_oversampling(uint8_t instance, uint8_t n)
{
_imu._gyro_over_sampling[instance] = n;
}
/*
update the sensor rate for FIFO sensors
@ -116,6 +128,9 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
{
float dt;
_update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance],
_imu._gyro_raw_sample_rates[instance]);
/*
we have two classes of sensors. FIFO based sensors produce data
at a very predictable overall rate, but the data comes in
@ -127,9 +142,6 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
if (sample_us != 0 && _imu._gyro_last_sample_us[instance] != 0) {
dt = (sample_us - _imu._gyro_last_sample_us[instance]) * 1.0e-6;
} else {
_update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance],
_imu._gyro_raw_sample_rates[instance]);
// don't accept below 100Hz
if (_imu._gyro_raw_sample_rates[instance] < 100) {
return;
@ -234,6 +246,9 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
{
float dt;
_update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
_imu._accel_raw_sample_rates[instance]);
/*
we have two classes of sensors. FIFO based sensors produce data
at a very predictable overall rate, but the data comes in
@ -245,9 +260,6 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
if (sample_us != 0 && _imu._accel_last_sample_us[instance] != 0) {
dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6;
} else {
_update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
_imu._accel_raw_sample_rates[instance]);
// don't accept below 100Hz
if (_imu._accel_raw_sample_rates[instance] < 100) {
return;

6
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -125,6 +125,12 @@ protected:
// sensors, and should be set to zero for FIFO based sensors
void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0, bool fsync_set=false);
// set the amount of oversamping a accel is doing
void _set_accel_oversampling(uint8_t instance, uint8_t n);
// set the amount of oversamping a gyro is doing
void _set_gyro_oversampling(uint8_t instance, uint8_t n);
// update the sensor rate for FIFO sensors
void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz);

4
libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp
View File

@ -810,6 +810,10 @@ void AP_InertialSensor_Invensense::_set_filter_register(void)
_fast_sampling = (_mpu_type != Invensense_MPU6000 && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
if (_fast_sampling) {
hal.console->printf("MPU[%u]: enabled fast sampling\n", _accel_instance);
// for logging purposes set the oversamping rate
_set_accel_oversampling(_accel_instance, MPU_FIFO_DOWNSAMPLE_COUNT/2);
_set_gyro_oversampling(_accel_instance, MPU_FIFO_DOWNSAMPLE_COUNT);
}
}