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: Clean up aiding mode logic using switch statements

Browse Source
This commit is contained in:
priseborough 2017-06-21 10:50:30 +10:00 committed by Randy Mackay
parent b847a6e38c
commit 08ddae8882
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -283,7 +283,8 @@ void NavEKF3_core::setAidingMode()
// check to see if we are starting or stopping aiding and set states and modes as required
if (PV_AidingMode != PV_AidingModePrev) {
// set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
if (PV_AidingMode == AID_NONE) {
switch (PV_AidingMode) {
case AID_NONE:
// We have ceased aiding
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 IMU%u stopped aiding",(unsigned)imu_index);
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
@ -303,7 +304,9 @@ void NavEKF3_core::setAidingMode()
// reset relative aiding sensor fusion activity status
flowFusionActive = false;
bodyVelFusionActive = false;
} else if (PV_AidingMode == AID_RELATIVE) {
break;
case AID_RELATIVE:
// We are doing relative position navigation where velocity errors are constrained, but position drift will occur
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index);
if (readyToUseOptFlow()) {
@ -317,7 +320,9 @@ void NavEKF3_core::setAidingMode()
}
posTimeout = true;
velTimeout = true;
} else if (PV_AidingMode == AID_ABSOLUTE) {
break;
case AID_ABSOLUTE:
if (readyToUseGPS()) {
// We are commencing aiding using GPS - this is the preferred method
posResetSource = GPS;
@ -340,6 +345,10 @@ void NavEKF3_core::setAidingMode()
lastPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms;
lastRngBcnPassTime_ms = imuSampleTime_ms;
break;
default:
break;
}
// Always reset the position and velocity when changing mode