Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -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 // check to see if we are starting or stopping aiding and set states and modes as required
if (PV_AidingMode != PV_AidingModePrev) { 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. // 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 // We have ceased aiding
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 IMU%u stopped aiding",(unsigned)imu_index); 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 // 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 // reset relative aiding sensor fusion activity status
flowFusionActive = false; flowFusionActive = false;
bodyVelFusionActive = 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 // 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); GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index);
if (readyToUseOptFlow()) { if (readyToUseOptFlow()) {
@ -317,7 +320,9 @@ void NavEKF3_core::setAidingMode()
} }
posTimeout = true; posTimeout = true;
velTimeout = true; velTimeout = true;
} else if (PV_AidingMode == AID_ABSOLUTE) { break;
case AID_ABSOLUTE:
if (readyToUseGPS()) { if (readyToUseGPS()) {
// We are commencing aiding using GPS - this is the preferred method // We are commencing aiding using GPS - this is the preferred method
posResetSource = GPS; posResetSource = GPS;
@ -340,6 +345,10 @@ void NavEKF3_core::setAidingMode()
lastPosPassTime_ms = imuSampleTime_ms; lastPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms;
lastRngBcnPassTime_ms = imuSampleTime_ms; lastRngBcnPassTime_ms = imuSampleTime_ms;
break;
default:
break;
} }
// Always reset the position and velocity when changing mode // Always reset the position and velocity when changing mode