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

AP_InertialSensor: synchronise fifo reads on the ICM42xxx

This commit is contained in:
Andy Piper 2022-08-03 10:27:34 +01:00 committed by Andrew Tridgell
parent 5eb2f6780e
commit 221ab9752e
3 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -316,6 +316,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
_imu._new_gyro_data[instance] = true; _imu._new_gyro_data[instance] = true;
} }
// 5us
if (!_imu.batchsampler.doing_post_filter_logging()) { if (!_imu.batchsampler.doing_post_filter_logging()) {
log_gyro_raw(instance, sample_us, gyro); log_gyro_raw(instance, sample_us, gyro);
} }
@ -543,6 +544,7 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
_imu._new_accel_data[instance] = true; _imu._new_accel_data[instance] = true;
} }
// 5us
if (!_imu.batchsampler.doing_post_filter_logging()) { if (!_imu.batchsampler.doing_post_filter_logging()) {
log_accel_raw(instance, sample_us, accel); log_accel_raw(instance, sample_us, accel);
} else { } else {

15
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -119,7 +119,7 @@ struct PACKED FIFOData {
}; };
#define INV3_SAMPLE_SIZE sizeof(FIFOData) #define INV3_SAMPLE_SIZE sizeof(FIFOData)
#define INV3_FIFO_BUFFER_LEN 8 #define INV3_FIFO_BUFFER_LEN 2
AP_InertialSensor_Invensensev3::AP_InertialSensor_Invensensev3(AP_InertialSensor &imu, AP_InertialSensor_Invensensev3::AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> _dev, AP_HAL::OwnPtr<AP_HAL::Device> _dev,
@ -230,6 +230,9 @@ void AP_InertialSensor_Invensensev3::start()
set_filter_and_scaling(); set_filter_and_scaling();
} }
// pre-calculate backend period
backend_period_us = 1000000UL / backend_rate_hz;
if (!_imu.register_gyro(gyro_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype)) || if (!_imu.register_gyro(gyro_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype)) ||
!_imu.register_accel(accel_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype))) { !_imu.register_accel(accel_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype))) {
return; return;
@ -257,7 +260,7 @@ void AP_InertialSensor_Invensensev3::start()
} }
// start the timer process to read samples, using the fastest rate avilable // start the timer process to read samples, using the fastest rate avilable
dev->register_periodic_callback(1000000UL / backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev3::read_fifo, void)); periodic_handle = dev->register_periodic_callback(backend_period_us, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev3::read_fifo, void));
} }
// get a startup banner to output to the GCS // get a startup banner to output to the GCS
@ -315,9 +318,9 @@ bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, ui
accel *= accel_scale; accel *= accel_scale;
gyro *= GYRO_SCALE; gyro *= GYRO_SCALE;
const float temp = d.temperature * temp_sensitivity + temp_zero; const float temp = d.temperature * temp_sensitivity + temp_zero;
// these four calls are about 40us
_rotate_and_correct_accel(accel_instance, accel); _rotate_and_correct_accel(accel_instance, accel);
_rotate_and_correct_gyro(gyro_instance, gyro); _rotate_and_correct_gyro(gyro_instance, gyro);
@ -336,9 +339,9 @@ void AP_InertialSensor_Invensensev3::read_fifo()
{ {
bool need_reset = false; bool need_reset = false;
uint16_t n_samples; uint16_t n_samples;
const uint8_t reg_counth = (inv3_type == Invensensev3_Type::ICM42670)?INV3REG_70_FIFO_COUNTH:INV3REG_FIFO_COUNTH; const uint8_t reg_counth = (inv3_type == Invensensev3_Type::ICM42670)?INV3REG_70_FIFO_COUNTH:INV3REG_FIFO_COUNTH;
const uint8_t reg_data = (inv3_type == Invensensev3_Type::ICM42670)?INV3REG_70_FIFO_DATA:INV3REG_FIFO_DATA; const uint8_t reg_data = (inv3_type == Invensensev3_Type::ICM42670)?INV3REG_70_FIFO_DATA:INV3REG_FIFO_DATA;
if (!block_read(reg_counth, (uint8_t*)&n_samples, 2)) { if (!block_read(reg_counth, (uint8_t*)&n_samples, 2)) {
goto check_registers; goto check_registers;
} }
@ -348,6 +351,10 @@ void AP_InertialSensor_Invensensev3::read_fifo()
goto check_registers; goto check_registers;
} }
// adjust the periodic callback to be synchronous with the incoming data
// this means that we rarely run read_fifo() without updating the sensor data
dev->adjust_periodic_callback(periodic_handle, backend_period_us);
while (n_samples > 0) { while (n_samples > 0) {
uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN); uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN);
if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * INV3_SAMPLE_SIZE)) { if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * INV3_SAMPLE_SIZE)) {

3
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
View File

@ -88,8 +88,11 @@ private:
// what rate are we generating samples into the backend for gyros and accels? // what rate are we generating samples into the backend for gyros and accels?
uint16_t backend_rate_hz; uint16_t backend_rate_hz;
// pre-calculated backend period
uint32_t backend_period_us;
AP_HAL::OwnPtr<AP_HAL::Device> dev; AP_HAL::OwnPtr<AP_HAL::Device> dev;
AP_HAL::Device::PeriodicHandle periodic_handle;
// which sensor type this is // which sensor type this is
enum Invensensev3_Type inv3_type; enum Invensensev3_Type inv3_type;