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

AP_InertialSensor: only update the primary once. scale FIFO reads to loop rate.

This commit is contained in:
Andy Piper 2024-12-21 20:18:15 +00:00
parent 9bc66372d7
commit ed0eb5924c
4 changed files with 6 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -817,8 +817,7 @@ void AP_InertialSensor_Backend::update_primary()
uint32_t now_us = AP_HAL::micros();
if (is_primary != is_new_primary
|| AP_HAL::timeout_expired(last_primary_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) {
set_primary_gyro(is_new_primary);
set_primary_accel(is_new_primary);
set_primary(is_new_primary);
is_primary = is_new_primary;
last_primary_update_us = now_us;
}

3
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -296,8 +296,7 @@ protected:
// catch updates to the primary gyro and accel
void update_primary() __RAMFUNC__; /* backend */
virtual void set_primary_gyro(bool is_primary_gyro) {}
virtual void set_primary_accel(bool is_primary_accel) {}
virtual void set_primary(bool _is_primary) {}
// support for updating filter at runtime
uint16_t _last_accel_filter_hz;

5
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -401,14 +401,15 @@ bool AP_InertialSensor_Invensensev3::update()
return true;
}
void AP_InertialSensor_Invensensev3::set_primary_gyro(bool _is_primary)
void AP_InertialSensor_Invensensev3::set_primary(bool _is_primary)
{
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
if (_imu.is_dynamic_fifo_enabled(gyro_instance)) {
if (_is_primary) {
dev->adjust_periodic_callback(periodic_handle, backend_period_us);
} else {
dev->adjust_periodic_callback(periodic_handle, backend_period_us * get_fast_sampling_rate());
// scale down non-primary to loop rate
dev->adjust_periodic_callback(periodic_handle, 1000000UL / get_loop_rate_hz());
}
}
#endif

2
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
View File

@ -46,7 +46,7 @@ public:
const uint16_t multiplier_accel = INT16_MAX/(32*GRAVITY_MSS);
protected:
void set_primary_gyro(bool is_primary) override;
void set_primary(bool _is_primary) override;
private:
AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,