Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-14 11:54:01 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: have a single callback for primary switching

Browse Source
This commit is contained in:
Andy Piper 2024-12-19 10:13:35 +00:00 committed by Andrew Tridgell
parent 45242de521
commit 32632a06a8
3 changed files with 18 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -370,7 +370,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
// 5us
log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]);
update_primary_gyro();
update_primary();
}
/*
@ -458,7 +458,7 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
}
log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]);
update_primary_gyro();
update_primary();
}
void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &raw_gyro, const Vector3f &filtered_gyro)
@ -616,7 +616,6 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
// assume we're doing pre-filter logging:
log_accel_raw(instance, sample_us, accel);
#endif
update_primary_accel();
}
/*
@ -694,7 +693,6 @@ void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, con
// assume we're doing pre-filter logging
log_accel_raw(instance, sample_us, accel);
#endif
update_primary_accel();
}
@ -810,17 +808,19 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
update_gyro_filters(instance);
}
void AP_InertialSensor_Backend::update_primary_gyro()
void AP_InertialSensor_Backend::update_primary()
{
// timing changes need to be made in the bus thread in order to take effect which is
// why they are actioned here
// why they are actioned here. Currently the _primary_gyro and _primary_accel can never
// be different for a particular IMU
const bool is_new_primary = gyro_instance == _imu._primary_gyro;
uint32_t now_us = AP_HAL::micros();
if (is_primary_gyro != is_new_primary
|| AP_HAL::timeout_expired(last_primary_gyro_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) {
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);
is_primary_gyro = is_new_primary;
last_primary_gyro_update_us = now_us;
set_primary_accel(is_new_primary);
is_primary = is_new_primary;
last_primary_update_us = now_us;
}
}
@ -867,20 +867,6 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */
update_accel_filters(instance);
}
void AP_InertialSensor_Backend::update_primary_accel()
{
// timing changes need to be made in the bus thread in order to take effect which is
// why they are actioned here
const bool is_new_primary = accel_instance == _imu._primary_accel;
uint32_t now_us = AP_HAL::micros();
if (is_primary_accel != is_new_primary
|| AP_HAL::timeout_expired(last_primary_accel_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) {
set_primary_accel(is_new_primary);
is_primary_accel = is_new_primary;
last_primary_accel_update_us = now_us;
}
}
/*
propagate filter changes from front end to backend
*/

13
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -160,11 +160,9 @@ protected:
// instance numbers of accel and gyro data
uint8_t gyro_instance;
bool is_primary_gyro = true;
uint32_t last_primary_gyro_update_us;
uint8_t accel_instance;
bool is_primary_accel = true;
uint32_t last_primary_accel_update_us;
bool is_primary = true;
uint32_t last_primary_update_us;
void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__;
void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__;
@ -297,10 +295,9 @@ protected:
void update_accel_filters(uint8_t instance) __RAMFUNC__; /* front end */
// catch updates to the primary gyro and accel
void update_primary_gyro() __RAMFUNC__; /* backend */
void update_primary_accel() __RAMFUNC__; /* backend */
virtual void set_primary_gyro(bool is_primary) {}
virtual void set_primary_accel(bool is_primary) {}
void update_primary() __RAMFUNC__; /* backend */
virtual void set_primary_gyro(bool is_primary_gyro) {}
virtual void set_primary_accel(bool is_primary_accel) {}
// support for updating filter at runtime
uint16_t _last_accel_filter_hz;

6
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -401,11 +401,11 @@ bool AP_InertialSensor_Invensensev3::update()
return true;
}
void AP_InertialSensor_Invensensev3::set_primary_gyro(bool is_primary)
void AP_InertialSensor_Invensensev3::set_primary_gyro(bool _is_primary)
{
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
if (_imu.is_dynamic_fifo_enabled(gyro_instance)) {
if (is_primary) {
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());
@ -570,7 +570,7 @@ void AP_InertialSensor_Invensensev3::read_fifo()
// adjust the periodic callback to be synchronous with the incoming data
// this means that we rarely run read_fifo() without updating the sensor data
if (is_primary_gyro) {
if (is_primary) {
dev->adjust_periodic_callback(periodic_handle, backend_period_us);
}