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
parent 32cc2c452c
commit d786937ea3
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)
{
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;