diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 7b962280e9..f6073ae4ba 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -283,6 +283,15 @@ void NavEKF3_core::setAidingMode() (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); } + // This is a special case for when we are a fixed wing aircraft vehicle that has landed and + // has no yaw measurement that works when static. Declare the yaw as unaligned (unknown) + // and declare attitude aiding loss so that we fall back into a non-aiding mode + if (assume_zero_sideslip() && onGround && (effectiveMagCal == MagCal::GSF_YAW) && !use_compass()){ + yawAlignComplete = false; + finalInflightYawInit = false; + attAidLossCritical = true; + } + if (attAidLossCritical) { // if the loss of attitude data is critical, then put the filter into a constant position mode PV_AidingMode = AID_NONE; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 0c9ff082c8..3c43759fc0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -343,24 +343,18 @@ void NavEKF3_core::detectFlight() largeHgtChange = true; } - // Determine to a high certainty we are flying - if (motorsArmed && highGndSpd && (highAirSpd || largeHgtChange)) { + if (motorsArmed) { onGround = false; - inFlight = true; - } - - // if is possible we are in flight, set the time this condition was last detected - if (motorsArmed && (highGndSpd || highAirSpd || largeHgtChange)) { - airborneDetectTime_ms = imuSampleTime_ms; - onGround = false; - } - - // Determine to a high certainty we are not flying - // after 5 seconds of not detecting a possible flight condition or we are disarmed, we transition to on-ground mode - if(!motorsArmed || ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) { + if (highGndSpd && (highAirSpd || largeHgtChange)) { + // to a high certainty we are flying + inFlight = true; + } + } else { + // to a high certainty we are not flying onGround = true; inFlight = false; } + } else { // Non fly forward vehicle, so can only use height and motor arm status diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 65d3eb6ee0..a0b5d51560 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1008,7 +1008,6 @@ private: bool inFlight; // true when the vehicle is definitely flying bool prevInFlight; // value inFlight from previous frame - used to detect transition bool manoeuvring; // boolean true when the flight vehicle is performing horizontal changes in velocity - uint32_t airborneDetectTime_ms; // last time flight movement was detected Vector6 innovVelPos; // innovation output for a group of measurements Vector6 varInnovVelPos; // innovation variance output for a group of measurements Vector6 velPosObs; // observations for combined velocity and positon group of measurements (3x1 m , 3x1 m/s)