Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Andrew Tridgell 2024-02-27 09:22:42 +11:00 committed by Peter Barker
parent 9770855c9d
commit 192d9f6dbd
1 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_AHRS/AP_AHRS_External.cpp
View File

@ -23,7 +23,9 @@ bool AP_AHRS_External::healthy() const {
void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results) void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results)
{ {
Quaternion quat; Quaternion quat;
if (!AP::externalAHRS().get_quaternion(quat)) { auto &extahrs = AP::externalAHRS();
const AP_InertialSensor &_ins = AP::ins();
if (!extahrs.get_quaternion(quat)) {
return; return;
} }
quat.rotation_matrix(results.dcm_matrix); 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.dcm_matrix.to_euler(&results.roll_rad, &results.pitch_rad, &results.yaw_rad);
results.gyro_drift.zero(); 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; const Vector3f accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;
results.accel_ef = accel_ef; results.accel_ef = accel_ef;