Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Latch use of position hold mode for duration of flight

Browse Source 
Required to prevent acquisition of GPS mid flight causing unwanted change in position and velocity
A distinction has been mad between the arm and disarm transition and the decision to use position hold mode (formerly static mode)
This commit is contained in:
priseborough 2014-12-21 20:41:40 +11:00 committed by Randy Mackay
parent a0a6c0362f
commit 363c1e393d
2 changed files with 44 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -374,7 +374,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
covDelAngMax(0.05f), // maximum delta angle between covariance prediction updates
TASmsecMax(200), // maximum allowed interval between airspeed measurement updates
posHoldMode(true), // forces position fusion with zero values
prevPosHoldMode(true), // posHoldMode from previous filter update
yawAligned(false), // set true when heading or yaw angle has been aligned
inhibitWindStates(true), // inhibit wind state updates on startup
inhibitMagStates(true) // inhibit magnetometer state updates on startup
@ -685,22 +684,18 @@ void NavEKF::UpdateFilter()
// check if on ground
SetFlightAndFusionModes();
// define rules used to set position hold mode
// position hold enables attitude only estimates without GPS by fusing zeros for position
if (vehicleNotArmed() || (_fusionModeGPS == 3 && !optFlowDataPresent())) {
posHoldMode = true;
} else {
posHoldMode = false;
}
// determine vehicle arm status
prevVehicleArmed = vehicleArmed;
vehicleArmed = getVehicleArmStatus();
// check to see if position hold mode has changed and reset states if it has
if (prevPosHoldMode != posHoldMode) {
// check to see if arm status has changed and reset states if it has
if (vehicleArmed != prevVehicleArmed) {
ResetVelocity();
ResetPosition();
ResetHeight();
StoreStatesReset();
// only reset the magnetic field and heading on the first arm. This prevents in-flight learning being forgotten for vehicles that do multiple short flights and disarm in-between.
if (!posHoldMode && !firstArmComplete) {
if (vehicleArmed && !firstArmComplete) {
firstArmComplete = true;
state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
firstArmPosD = state.position.z;
@ -708,29 +703,41 @@ void NavEKF::UpdateFilter()
// zero stored velocities used to do dead-reckoning
heldVelNE.zero();
// set various useage modes based on the condition at arming. These are then held until the vehicle is disarmed.
if (!prevPosHoldMode) {
gpsInhibitMode = 0; // When entering potion hold mode (dis-arming), clear any GPS mode inhibits
} else if (_fusionModeGPS == 3) { // GPS useage has been explicitly prohibited
if (!vehicleArmed) {
gpsInhibitMode = 0; // When dis-arming, clear any GPS mode inhibits
} else if (_fusionModeGPS == 3) { // arming when GPS useage has been prohibited
if (optFlowDataPresent()) {
gpsInhibitMode = 2; // we have optical flow data and can estimate all vehicle states
posTimeout = true;
velTimeout = true;
} else {
gpsInhibitMode = 1; // we don't have optical flow data and will only be able to estimate orientation and height
posTimeout = true;
velTimeout = true;
}
} else { //GPS useage is allowed
if ((imuSampleTime_ms - lastFixTime_ms) < 500) {
gpsInhibitMode = 0; // we have GPS data and can estimate all vehicle states
} else {
} else { // arming when GPS useage is allowed
if (gpsNotAvailable) {
gpsInhibitMode = 1; // we don't have have GPS data and will only be able to estimate orientation and height
posTimeout = true;
velTimeout = true;
} else {
gpsInhibitMode = 0; // we have GPS data and can estimate all vehicle states
}
}
prevPosHoldMode = posHoldMode;
} else if (!posHoldMode && !finalMagYawInit && firstArmPosD - state.position.z > 1.5f && !assume_zero_sideslip()) {
} else if (vehicleArmed && !finalMagYawInit && firstArmPosD - state.position.z > 1.5f && !assume_zero_sideslip()) {
// Do a final yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
finalMagYawInit = true;
}
// define rules used to set position hold mode
// position hold enables attitude only estimates without GPS by fusing zeros for position
if ((vehicleArmed && gpsInhibitMode == 1) || !vehicleArmed) {
posHoldMode = true;
} else {
posHoldMode = false;
}
// run the strapdown INS equations every IMU update
UpdateStrapdownEquationsNED();
@ -4017,6 +4024,12 @@ void NavEKF::readGpsData()
// calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
decayGpsOffset();
}
// If too long since last fix time, we declare no GPS present
if (imuSampleTime_ms - lastFixTime_ms < 1000) {
gpsNotAvailable = false;
} else {
gpsNotAvailable = true;
}
}
// check for new altitude measurement data and update stored measurement if available
@ -4397,6 +4410,8 @@ void NavEKF::ZeroVariables()
heldVelNE.zero();
gpsInhibitMode = 0;
gpsVelGlitchOffset.zero();
vehicleArmed = false;
prevVehicleArmed = false;
}
// return true if we should use the airspeed sensor
@ -4425,10 +4440,10 @@ bool NavEKF::optFlowDataPresent(void) const
}
}
// return true if the vehicle is not armed or requesting a static vehicle assumption for correction of attitude errors
bool NavEKF::vehicleNotArmed(void) const
// return true if the vehicle is requesting the filter to be ready for flight
bool NavEKF::getVehicleArmStatus(void) const
{
return !_ahrs->get_armed() || !_ahrs->get_correct_centrifugal();
return _ahrs->get_armed() || _ahrs->get_correct_centrifugal();
}
// return true if we should use the compass

12
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -157,9 +157,6 @@ public:
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
void getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const;
// return StaticMode state
bool getStaticMode(void) const { return posHoldMode; }
// should we use the compass? This is public so it can be used for
// reporting via ahrs.use_compass()
bool use_compass(void) const;
@ -354,9 +351,8 @@ private:
// return true if we should use the airspeed sensor
bool useAirspeed(void) const;
// return true if the vehicle code has requested operation in a pre-armed state where GPS data isnt used to correct attitude
// this causes operation in a mode producing attitude and height only
bool vehicleNotArmed(void) const;
// return true if the vehicle code has requested the filter to be ready for flight
bool getVehicleArmStatus(void) const;
// decay GPS horizontal position offset to close to zero at a rate of 1 m/s
// this allows large GPS position jumps to be accomodated gradually
@ -522,7 +518,6 @@ private:
uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step
bool inhibitLoadLeveling; // boolean that turns off delay of fusion to level processor loading
bool posHoldMode; // boolean to force position measurements to zero for operation without GPS
bool prevPosHoldMode; // value of static mode from last update
uint32_t lastMagUpdate; // last time compass was updated
Vector3f velDotNED; // rate of change of velocity in NED frame
Vector3f velDotNEDfilt; // low pass filtered velDotNED
@ -571,6 +566,9 @@ private:
bool finalMagYawInit; // true when the final post takeoff initialisation of earth field and yaw angle has been performed
bool flowTimeout; // true when optical flow measurements have time out
Vector2f gpsVelGlitchOffset; // Offset applied to the GPS velocity when the gltch radius is being decayed back to zero
bool gpsNotAvailable; // bool true when valid GPS data is not available
bool vehicleArmed; // true when the vehicle is disarmed
bool prevVehicleArmed; // vehicleArmed from previous frame
// Used by smoothing of state corrections
float gpsIncrStateDelta[10]; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement