From 6eb02b364ea20ea9bb67b4a4b9c8b2b88f0a4540 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 10 Jul 2024 13:26:42 +0100 Subject: [PATCH] AP_InertialSensor: directly publish gyro values so that AHRS sees the latest --- .../AP_InertialSensor/AP_InertialSensor.h | 5 ++++ .../AP_InertialSensor_Backend.cpp | 30 ++++++++++++++----- 2 files changed, 27 insertions(+), 8 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index b42ffb3f8b61f8..a0ad58abf2fd12 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -821,7 +821,12 @@ class AP_InertialSensor : AP_AccelCal_Client bool get_next_gyro_sample(Vector3f& gyro); uint32_t get_num_gyro_samples() { return _rate_loop_gyro_window.available(); } void set_rate_decimation(uint8_t rdec) { rate_decimation = rdec; } + // are gyro samples being sourced from the rate loop buffer bool use_rate_loop_gyro_samples() const { return _cmutex != nullptr; } + // whether or not to push the current gyro sample + bool push_rate_loop_gyro(uint8_t instance) const { + return use_rate_loop_gyro_samples() && rate_decimation && instance == _primary_gyro; + } void update_backend_filters(); bool gyro_samples_available() { return _rate_loop_gyro_window.available() > 0; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 9df8f196416fbb..7a98efeaf6330f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -174,9 +174,20 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f & if (has_been_killed(instance)) { return; } - _imu._gyro[instance] = gyro; - _imu._gyro_healthy[instance] = true; +#if AP_INERTIALSENSOR_RATE_LOOP_WINDOW_ENABLED + if (!_imu.push_rate_loop_gyro(instance)) { // rate loop thread is not consuming samples +#endif + _imu._gyro[instance] = gyro; +#if HAL_GYROFFT_ENABLED + // copy the gyro samples from the backend to the frontend window for FFTs sampling at less than IMU rate + _imu._gyro_for_fft[instance] = _imu._last_gyro_for_fft[instance]; +#endif +#if AP_INERTIALSENSOR_RATE_LOOP_WINDOW_ENABLED + } +#endif + + _imu._gyro_healthy[instance] = true; // publish delta angle _imu._delta_angle[instance] = _imu._delta_angle_acc[instance]; _imu._delta_angle_dt[instance] = _imu._delta_angle_acc_dt[instance]; @@ -263,16 +274,22 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const } #if AP_INERTIALSENSOR_RATE_LOOP_WINDOW_ENABLED - if (_imu.use_rate_loop_gyro_samples() && _imu.rate_decimation && instance == _imu._primary_gyro) { + if (_imu.push_rate_loop_gyro(instance)) { /* tell the rate thread we have a new sample */ if (++_imu.rate_decimation_count >= _imu.rate_decimation) { _imu._cmutex->lock_and_signal(); - if (!_imu._rate_loop_gyro_window.push(gyro_filtered)) { + if (!_imu._rate_loop_gyro_window.push(_imu._gyro_filtered[instance])) { debug("dropped rate loop sample"); } _imu.rate_decimation_count = 0; + // semaphore is already held so we can directly publish the gyro data + _imu._gyro[instance] = _imu._gyro_filtered[instance]; +#if HAL_GYROFFT_ENABLED + // copy the gyro samples from the backend to the frontend window for FFTs sampling at less than IMU rate + _imu._gyro_for_fft[instance] = _imu._last_gyro_for_fft[instance]; +#endif _imu._cmutex->unlock(); } } @@ -792,12 +809,9 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */ if (has_been_killed(instance)) { return; } + if (_imu._new_gyro_data[instance]) { _publish_gyro(instance, _imu._gyro_filtered[instance]); -#if HAL_GYROFFT_ENABLED - // copy the gyro samples from the backend to the frontend window for FFTs sampling at less than IMU rate - _imu._gyro_for_fft[instance] = _imu._last_gyro_for_fft[instance]; -#endif _imu._new_gyro_data[instance] = false; }