AP_AHRS: don't use accel/gyro from ExternalAHRS unless enabled

ArduPilot · Feb 26, 2024 · e2e9003 · e2e9003
1 parent 91f60be
commit e2e9003
Showing 1 changed file with 11 additions and 3 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS_External.cpp b/libraries/AP_AHRS/AP_AHRS_External.cpp
@@ -23,17 +23,25 @@ bool AP_AHRS_External::healthy() const {
 void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results)
 {
     Quaternion quat;
-    if (!AP::externalAHRS().get_quaternion(quat)) {
+    auto &extahrs = AP::externalAHRS();
+    const AP_InertialSensor &_ins = AP::ins();
+    if (!extahrs.get_quaternion(quat)) {
         return;
     }
     quat.rotation_matrix(results.dcm_matrix);
     results.dcm_matrix = results.dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();
     results.dcm_matrix.to_euler(&results.roll_rad, &results.pitch_rad, &results.yaw_rad);
 
     results.gyro_drift.zero();
-    results.gyro_estimate = AP::externalAHRS().get_gyro();
+    if (!extahrs.get_gyro(results.gyro_estimate)) {
+        results.gyro_estimate = _ins.get_gyro();
+    }
+
+    Vector3f accel;
+    if (!extahrs.get_accel(accel)) {
+        accel = _ins.get_accel();
+    }
 
-    const Vector3f accel = AP::externalAHRS().get_accel();
     const Vector3f accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;
     results.accel_ef = accel_ef;