AP_InertialSensor: directly publish gyro values so that AHRS sees the…

… latest
ArduPilot · Jul 10, 2024 · 4f28521 · 4f28521
1 parent 6e235f0
commit 4f28521
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 8 deletions.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -819,7 +819,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
@@ -174,9 +174,20 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &
     if ((1U<<instance) & _imu.imu_kill_mask) {
         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 ((1U<<instance) & _imu.imu_kill_mask) {
         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;
     }