diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 2effc2d932..f6dac0b8ff 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -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; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 9c8be48b29..6afc0a1143 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -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