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

AP_NavEKF3: Handle repeated FW flight without magnetometer

Browse Source 
The EKF can build up large yaw errors on ground so it is safer to stop using GPS and re-align after launch as per first launch.
This commit is contained in:
Paul Riseborough 2020-06-14 17:10:13 +10:00 committed by Peter Barker
parent 8ff6780323
commit 354b551ef0
3 changed files with 17 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -283,6 +283,15 @@ void NavEKF3_core::setAidingMode()
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); (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 (attAidLossCritical) {
// if the loss of attitude data is critical, then put the filter into a constant position mode // if the loss of attitude data is critical, then put the filter into a constant position mode
PV_AidingMode = AID_NONE; PV_AidingMode = AID_NONE;

22
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -343,24 +343,18 @@ void NavEKF3_core::detectFlight()
largeHgtChange = true; largeHgtChange = true;
} }
// Determine to a high certainty we are flying if (motorsArmed) {
if (motorsArmed && highGndSpd && (highAirSpd || largeHgtChange)) {
onGround = false; onGround = false;
inFlight = true; if (highGndSpd && (highAirSpd || largeHgtChange)) {
} // to a high certainty we are flying
inFlight = true;
// if is possible we are in flight, set the time this condition was last detected }
if (motorsArmed && (highGndSpd || highAirSpd || largeHgtChange)) { } else {
airborneDetectTime_ms = imuSampleTime_ms; // to a high certainty we are not flying
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)) {
onGround = true; onGround = true;
inFlight = false; inFlight = false;
} }
} else { } else {
// Non fly forward vehicle, so can only use height and motor arm status // Non fly forward vehicle, so can only use height and motor arm status

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1008,7 +1008,6 @@ private:
bool inFlight; // true when the vehicle is definitely flying bool inFlight; // true when the vehicle is definitely flying
bool prevInFlight; // value inFlight from previous frame - used to detect transition 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 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 innovVelPos; // innovation output for a group of measurements
Vector6 varInnovVelPos; // innovation variance 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) Vector6 velPosObs; // observations for combined velocity and positon group of measurements (3x1 m , 3x1 m/s)