From 100dbec831630c54d42e8321807fc2d311aa64f0 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 19 Dec 2024 10:13:35 +0000 Subject: [PATCH] AP_InertialSensor: have a single callback for primary switching --- .../AP_InertialSensor_Backend.cpp | 34 ++++++------------- .../AP_InertialSensor_Backend.h | 13 +++---- .../AP_InertialSensor_Invensensev3.cpp | 6 ++-- 3 files changed, 18 insertions(+), 35 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index f6593840582c83..eb2e4fb89b7224 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -361,7 +361,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(); } /* @@ -449,7 +449,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) @@ -607,7 +607,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(); } /* @@ -685,7 +684,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(); } @@ -801,17 +799,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; } } @@ -858,20 +858,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 */ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index ca5e672b7da93c..17626518d912e9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -160,11 +160,9 @@ class AP_InertialSensor_Backend // 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 @@ class AP_InertialSensor_Backend 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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index 2bf21b7f8a2a7a..bb7aad61f6cc52 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -406,11 +406,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()); @@ -575,7 +575,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); }