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

AP_NavEKF3: Fix bug preventing use of default or synthetic airspeed

This commit is contained in:
Paul Riseborough 2023-09-26 19:36:39 +10:00 committed by Andrew Tridgell
parent 2b027a7265
commit 568972c0f0
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -868,7 +868,7 @@ void NavEKF3_core::readAirSpdData()
// Allow use of a default value or a value synthesised from a stored wind velocity vector
if (!useAirspeed()) {
if (is_positive(defaultAirSpeed)) {
if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200) {
if (imuDataDelayed.time_ms - lastTasPassTime_ms > 200) {
tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f)));
tasDataToFuse = true;
@ -877,7 +877,7 @@ void NavEKF3_core::readAirSpdData()
tasDataToFuse = false;
}
} else if (windStateLastObsIsValid && !windStateIsObservable) {
if (imuDataDelayed.time_ms - tasDataDelayed.time_ms > 200) {
if (imuDataDelayed.time_ms - lastTasPassTime_ms > 200) {
// use stored wind state to synthesise an airspeed measurement
const Vector3F windRelVel = stateStruct.velocity - Vector3F(windStateLastObs.x, windStateLastObs.y, 0.0F);
tasDataDelayed.tas = windRelVel.length();