From 192d9f6dbd4494bfee734468c855dcb41c2f802e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 27 Feb 2024 09:22:42 +1100 Subject: [PATCH] AP_AHRS: don't use accel/gyro from ExternalAHRS unless enabled --- libraries/AP_AHRS/AP_AHRS_External.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_External.cpp b/libraries/AP_AHRS/AP_AHRS_External.cpp index 8cdf8ecb52..435bda69b4 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.cpp +++ b/libraries/AP_AHRS/AP_AHRS_External.cpp @@ -23,7 +23,9 @@ 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); @@ -31,9 +33,15 @@ void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results) 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;