aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Select startup aiding mode after landing without yaw sensor

Browse Source
This commit is contained in:
Paul Riseborough 2020-12-12 07:22:44 +11:00 committed by Andrew Tridgell
parent f5f13b9a47
commit b92eca7b66
1 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -206,6 +206,19 @@ void NavEKF3_core::setAidingMode()
// Check that the gyro bias variance has converged // Check that the gyro bias variance has converged
checkGyroCalStatus(); checkGyroCalStatus();
// Handle the special case where we are on ground and disarmed without a yaw measurement
// and navigating. This can occur if not using a magnetometer and yaw was aligned using GPS
// during the previous flight.
if (yaw_source_last == AP_NavEKF_Source::SourceYaw::NONE &&
!motorsArmed &&
onGround &&
PV_AidingMode != AID_NONE)
{
PV_AidingMode = AID_NONE;
yawAlignComplete = false;
finalInflightYawInit = false;
}
// Determine if we should change aiding mode // Determine if we should change aiding mode
switch (PV_AidingMode) { switch (PV_AidingMode) {
case AID_NONE: { case AID_NONE: {
@ -286,15 +299,6 @@ 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 && !use_compass() && !using_external_yaw()) {
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;