Browse Source

AP_NavEKF: Consistently set timeout flags whenever aiding is inhibited

This ensures the position and velocity measurement status will be set as timed out immediately after use of those measurements is inhibited. This will improve the timeliness of filter status reporting.
mission-4.1.18
priseborough 10 years ago committed by Randy Mackay
parent
commit
d2da16e652
  1. 10
      libraries/AP_NavEKF/AP_NavEKF.cpp

10
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -748,6 +748,8 @@ void NavEKF::SelectVelPosFusion() @@ -748,6 +748,8 @@ void NavEKF::SelectVelPosFusion()
constVelMode = false; // always clear constant velocity mode if constant velocity mode is active
constPosMode = true;
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
// reset the velocity
ResetVelocity();
// store the current position to be used to keep reporting the last known position
@ -4309,8 +4311,6 @@ void NavEKF::InitialiseVariables() @@ -4309,8 +4311,6 @@ void NavEKF::InitialiseVariables()
// initialise other variables
gpsNoiseScaler = 1.0f;
velTimeout = true;
posTimeout = true;
hgtTimeout = true;
magTimeout = true;
tasTimeout = true;
@ -4363,6 +4363,8 @@ void NavEKF::InitialiseVariables() @@ -4363,6 +4363,8 @@ void NavEKF::InitialiseVariables()
lastConstVelMode = false;
heldVelNE.zero();
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
gpsVelGlitchOffset.zero();
vehicleArmed = false;
prevVehicleArmed = false;
@ -4561,6 +4563,8 @@ void NavEKF::performArmingChecks() @@ -4561,6 +4563,8 @@ void NavEKF::performArmingChecks()
// set various useage modes based on the condition at arming. These are then held until the vehicle is disarmed.
if (!vehicleArmed) {
PV_AidingMode = AID_NONE; // When dis-armed, we only estimate orientation & height using the constant position mode
posTimeout = true;
velTimeout = true;
constPosMode = true;
constVelMode = false; // always clear constant velocity mode if constant position mode is active
lastConstVelMode = false;
@ -4599,6 +4603,8 @@ void NavEKF::performArmingChecks() @@ -4599,6 +4603,8 @@ void NavEKF::performArmingChecks()
// Always turn aiding off when the vehicle is disarmed
if (!vehicleArmed) {
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
// set constant position mode if aiding is inhibited
constPosMode = true;
constVelMode = false; // always clear constant velocity mode if constant position mode is active

Loading…
Cancel
Save