Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Improve inFlight detection for fixed wing

Browse Source 
Better handle case where no airspeed sensor is being used with a hand launch.
This commit is contained in:
Paul Riseborough 2021-01-03 08:53:28 +11:00 committed by Andrew Tridgell
parent 4e648734fa
commit 85c0040fa3
2 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -305,7 +305,7 @@ void NavEKF3_core::detectFlight()
if (assume_zero_sideslip()) {
// To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change
float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);
float gndSpdSq = sq(gpsDataNew.vel.x) + sq(gpsDataNew.vel.y);
bool highGndSpd = false;
bool highAirSpd = false;
bool largeHgtChange = false;
@ -318,8 +318,9 @@ void NavEKF3_core::detectFlight()
}
}
// trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors
if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) {
// trigger on ground speed
const float gndSpdThresholdSq = sq(5.0f);
if (gndSpdSq > gndSpdThresholdSq + sq(gpsSpdAccuracy)) {
highGndSpd = true;
}
@ -330,7 +331,7 @@ void NavEKF3_core::detectFlight()
if (motorsArmed) {
onGround = false;
if (highGndSpd && (highAirSpd || largeHgtChange)) {
if (highGndSpd && (expectTakeoff || highAirSpd || largeHgtChange)) {
// to a high certainty we are flying
inFlight = true;
}

9
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -896,6 +896,15 @@ void NavEKF3_core::calcOutputStates()
posOffsetNED.zero();
}
// Detect fixed wing launch acceleration using latest data from IMU to enable early startup of filter functions
// that use launch acceleration to detect start of flight
if (!inFlight && !expectTakeoff && assume_zero_sideslip()) {
const float launchDelVel = imuDataNew.delVel.x + GRAVITY_MSS * imuDataNew.delVelDT * Tbn_temp.c.x;
if (launchDelVel > GRAVITY_MSS * imuDataNew.delVelDT) {
setTakeoffExpected(true);
}
}
// store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer
if (runUpdates) {
// store the states at the output time horizon