Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: ensure extnav angle error is at least 5deg

Browse Source
This commit is contained in:
Randy Mackay 2020-05-13 17:54:38 +09:00 committed by Andrew Tridgell
parent e9b939ccb5
commit 45e6896d95
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -978,7 +978,10 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
// extract yaw from the attitude
float roll_rad, pitch_rad, yaw_rad;
quat.to_euler(roll_rad, pitch_rad, yaw_rad);
writeEulerYawAngle(yaw_rad, angErr, timeStamp_ms, 2);
// ensure yaw accuracy is no better than 5 degrees (some callers may send zero)
const float yaw_accuracy_rad = MAX(angErr, radians(5.0f));
writeEulerYawAngle(yaw_rad, yaw_accuracy_rad, timeStamp_ms, 2);
storedExtNav.push(extNavDataNew);
}