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_NavEKF2: Clean up mode change logic

Browse Source
This commit is contained in:
Paul Riseborough 2015-09-30 21:52:09 +02:00 committed by Randy Mackay
parent ef624199f9
commit 325f4139fe
4 changed files with 211 additions and 206 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -385,7 +385,6 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
magFailTimeLimit_ms(10000), // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
magVarRateScale(0.05f), // scale factor applied to magnetometer variance due to angular rate
gyroBiasNoiseScaler(2.0f), // scale factor applied to imu gyro bias learning before the vehicle is armed
accelBiasNoiseScaler(1.0f), // scale factor applied to imu accel bias learning before the vehicle is armed
hgtAvg_ms(100), // average number of msec between height measurements
betaAvg_ms(100), // average number of msec between synthetic sideslip measurements
covTimeStepMax(0.07f), // maximum time (sec) between covariance prediction updates

1
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -275,7 +275,6 @@ private:
const uint32_t magFailTimeLimit_ms; // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
const float magVarRateScale; // scale factor applied to magnetometer variance due to angular rate
const float gyroBiasNoiseScaler; // scale factor applied to gyro bias state process noise when on ground
const float accelBiasNoiseScaler; // scale factor applied to accel bias state process noise when on ground
const uint16_t hgtAvg_ms; // average number of msec between height measurements
const uint16_t betaAvg_ms; // average number of msec between synthetic sideslip measurements
const float covTimeStepMax; // maximum time (sec) between covariance prediction updates

354
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -69,7 +69,7 @@ bool NavEKF2_core::healthy(void) const
}
// barometer and position innovations must be within limits when on-ground
float horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
if (!filterArmed && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) {
if (!motorsArmed && (fabsf(innovVelPos[5]) > 1.0f || horizErrSq > 1.0f)) {
return false;
}
@ -229,15 +229,12 @@ void NavEKF2_core::UpdateFilter()
//get starting time for update step
imuSampleTime_ms = hal.scheduler->millis();
// Check arm status and perform required checks and mode changes
controlFilterModes();
// read IMU data and convert to delta angles and velocities
readIMUData();
// check if on ground
SetFlightAndFusionModes();
// Check arm status and perform required checks and mode changes
performArmingChecks();
// run the strapdown INS equations every IMU update
UpdateStrapdownEquationsNED();
@ -263,25 +260,6 @@ void NavEKF2_core::UpdateFilter()
// Update states using optical flow data
SelectFlowFusion();
// Check for tilt convergence
float alpha = 1.0f*dtIMUavg;
float temp=tiltErrVec.length();
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
tiltAlignComplete = true;
hal.console->printf("EKF tilt alignment complete\n");
}
// once tilt has converged, align yaw using magnetic field measurements
if (tiltAlignComplete && !yawAlignComplete) {
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
StoreQuatReset();
yawAlignComplete = true;
hal.console->printf("EKF yaw alignment complete\n");
}
// Update states using magnetometer data
SelectMagFusion();
@ -446,11 +424,12 @@ void NavEKF2_core::SelectFlowFusion()
// Perform tilt check
bool tiltOK = (Tnb_flow.c.z > frontend.DCM33FlowMin);
// Constrain measurements to zero if we are using optical flow and are on the ground
if (frontend._fusionModeGPS == 3 && !takeOffDetected && filterArmed) {
if (frontend._fusionModeGPS == 3 && !takeOffDetected && isAiding) {
ofDataDelayed.flowRadXYcomp.zero();
ofDataDelayed.flowRadXY.zero();
flowDataValid = true;
}
// If the flow measurements have been rejected for too long and we are relying on them, then revert to constant position mode
if ((flowSensorTimeout || flowFusionTimeout) && PV_AidingMode == AID_RELATIVE) {
PV_AidingMode = AID_NONE;
@ -462,6 +441,7 @@ void NavEKF2_core::SelectFlowFusion()
// reset the position
ResetPosition();
}
// if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
// we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference
if ((newDataFlow || newDataRng) && tiltOK) {
@ -767,8 +747,6 @@ void NavEKF2_core::CovariancePrediction()
processNoise[15] = dVelBiasSigma;
if (expectGndEffectTakeoff) {
processNoise[15] = 0.0f;
} else if (!filterArmed) {
processNoise[15] = dVelBiasSigma * frontend.accelBiasNoiseScaler;
} else {
processNoise[15] = dVelBiasSigma;
}
@ -1307,7 +1285,7 @@ void NavEKF2_core::FuseVelPosNED()
R_OBS[5] = sq(constrain_float(frontend._baroAltNoise, 0.1f, 10.0f));
// reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
if ((getTakeoffExpected() || getTouchdownExpected()) && filterArmed) {
if ((getTakeoffExpected() || getTouchdownExpected()) && motorsArmed) {
R_OBS[5] *= frontend.gndEffectBaroScaler;
}
@ -1876,7 +1854,7 @@ void NavEKF2_core::FuseMagnetometer()
}
// If in the air and there is no other form of heading reference or we are yawing rapidly which creates larger inertial yaw errors,
// we strengthen the magnetometer attitude correction
if (filterArmed && ((PV_AidingMode == AID_NONE) || highYawRate) && j <= 3) {
if (motorsArmed && ((PV_AidingMode == AID_NONE) || highYawRate) && j <= 3) {
Kfusion[j] *= 4.0f;
}
statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
@ -2721,7 +2699,7 @@ void NavEKF2_core::EstimateTerrainOffset()
float losRateSq = velHorizSq / sq(heightAboveGndEst);
// don't update terrain offset state if there is no range finder and not generating enough LOS rate, or without GPS, as it is poorly observable
if (!fuseRngData && (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f || onGround)) {
if (!fuseRngData && (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f)) {
inhibitGndState = true;
} else {
inhibitGndState = false;
@ -3302,7 +3280,7 @@ bool NavEKF2_core::resetHeightDatum(void)
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
uint8_t NavEKF2_core::setInhibitGPS(void)
{
if(!filterArmed) {
if(!isAiding) {
return 0;
}
if (optFlowDataPresent()) {
@ -3442,79 +3420,6 @@ void NavEKF2_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn
gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset()
}
// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
void NavEKF2_core::SetFlightAndFusionModes()
{
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f) {
manoeuvring = true;
} else {
manoeuvring = false;
}
// if we are a fly forward type vehicle, then in-air mode can be determined through a combination of speed and height criteria
if (assume_zero_sideslip()) {
// Evaluate a numerical score that defines the likelihood we are in the air
float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);
bool highGndSpd = false;
bool highAirSpd = false;
bool largeHgtChange = false;
// trigger at 8 m/s airspeed
if (_ahrs->airspeed_sensor_enabled()) {
const AP_Airspeed *airspeed = _ahrs->get_airspeed();
if (airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 10.0f) {
highAirSpd = true;
}
}
// trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors
if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) {
highGndSpd = true;
}
// trigger if more than 10m away from initial height
if (fabsf(baroDataDelayed.hgt) > 10.0f) {
largeHgtChange = true;
}
// to go to in-air mode we also need enough GPS velocity to be able to calculate a reliable ground track heading and either a lerge height or airspeed change
if (onGround && highGndSpd && (highAirSpd || largeHgtChange)) {
onGround = false;
}
// if is possible we are in flight, set the time this condition was last detected
if (highGndSpd || highAirSpd || largeHgtChange) {
airborneDetectTime_ms = imuSampleTime_ms;
}
// after 5 seconds of not detecting a possible flight condition, we transition to on-ground mode
if(!onGround && ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
onGround = true;
}
// perform a yaw alignment check against GPS if exiting on-ground mode, bu tonly if we have enough ground speed
// this is done to protect against unrecoverable heading alignment errors due to compass faults
if (!onGround && prevOnGround) {
alignYawGPS();
}
}
// store current on-ground status for next time
prevOnGround = onGround;
// If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE));
// request mag calibration for both in-air and manoeuvre threshold options
bool magCalRequested = ((frontend._magCal == 0) && !onGround) || ((frontend._magCal == 1) && manoeuvring) || (frontend._magCal == 3);
// deny mag calibration request if we aren't using the compass, are in the pre-arm constant position mode or it has been inhibited by the user
bool magCalDenied = !use_compass() || (PV_AidingMode == AID_NONE) || (frontend._magCal == 2);
// inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied);
if (inhibitMagStates && inhibitWindStates) {
stateIndexLim = 15;
} else if (inhibitWindStates) {
stateIndexLim = 21;
} else {
stateIndexLim = 23;
}
}
// initialise the covariance matrix
void NavEKF2_core::CovarianceInit()
{
@ -3750,7 +3655,7 @@ void NavEKF2_core::readGpsData()
// We are by definition at the origin at the instant of alignment so set NE position to zero
gpsDataNew.pos.zero();
// If GPS useage isn't explicitly prohibited, we switch to absolute position mode
if (filterArmed && frontend._fusionModeGPS != 3) {
if (isAiding && frontend._fusionModeGPS != 3) {
PV_AidingMode = AID_ABSOLUTE;
// Initialise EKF position and velocity states
ResetPosition();
@ -3849,7 +3754,7 @@ void NavEKF2_core::readHgtData()
// calculate offset to baro data that enables baro to be used as a backup
// filter offset to reduce effect of baro noise and other transient errors on estimate
baroHgtOffset = 0.1f * (_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
} else if (filterArmed && takeOffDetected) {
} else if (isAiding && takeOffDetected) {
// use baro measurement and correct for baro offset - failsafe use only as baro will drift
baroDataNew.hgt = max(_baro.get_altitude() - baroHgtOffset, rngOnGnd);
} else {
@ -3871,7 +3776,7 @@ void NavEKF2_core::readHgtData()
const float dtBaro = frontend.hgtAvg_ms*1.0e-3f;
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
} else if (filterArmed && getTakeoffExpected()) {
} else if (isAiding && getTakeoffExpected()) {
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
baroDataNew.hgt = max(baroDataNew.hgt, meaHgtAtTakeOff);
@ -4201,7 +4106,6 @@ void NavEKF2_core::InitialiseVariables()
tasTimeout = true;
badMag = false;
badIMUdata = false;
firstArmComplete = false;
firstMagYawInit = false;
secondMagYawInit = false;
dtIMUavg = 0.0025f;
@ -4231,8 +4135,8 @@ void NavEKF2_core::InitialiseVariables()
posTimeout = true;
velTimeout = true;
gpsVelGlitchOffset.zero();
filterArmed = false;
prevFilterArmed = false;
isAiding = false;
prevIsAiding = false;
memset(&faultStatus, 0, sizeof(faultStatus));
hgtRate = 0.0f;
mag_state.q0 = 1;
@ -4269,7 +4173,7 @@ void NavEKF2_core::InitialiseVariables()
velCorrection.zero();
gpsQualGood = false;
gpsNotAvailable = true;
motorsArmed = false;
}
// return true if we should use the airspeed sensor
@ -4294,7 +4198,7 @@ bool NavEKF2_core::optFlowDataPresent(void) const
// return true if the filter to be ready to use gps
bool NavEKF2_core::readyToUseGPS(void) const
{
return validOrigin && tiltAlignComplete && yawAlignComplete;
return validOrigin && tiltAlignComplete && yawAlignComplete && gpsQualGood;
}
// return true if we should use the compass
@ -4487,73 +4391,191 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan)
}
// Check arm status and perform required checks and mode changes
void NavEKF2_core::performArmingChecks()
// Control filter mode transitions
void NavEKF2_core::controlFilterModes()
{
// don't allow filter to arm until it has been running for long enough to stabilise
prevFilterArmed = filterArmed;
filterArmed = ((readyToUseGPS() || frontend._fusionModeGPS == 3) && (imuSampleTime_ms - ekfStartTime_ms) > 1000);
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f) {
manoeuvring = true;
} else {
manoeuvring = false;
}
// check to see if arm status has changed and reset states if it has
if (filterArmed != prevFilterArmed) {
// 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 (filterArmed && !firstArmComplete) {
firstArmComplete = true;
// Detect if we are in flight
detectFlight();
// If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE));
// request mag calibration for both in-air and manoeuvre threshold options
bool magCalRequested = ((frontend._magCal == 0) && !onGround) || ((frontend._magCal == 1) && manoeuvring) || (frontend._magCal == 3);
// deny mag calibration request if we aren't using the compass, are in the pre-arm constant position mode or it has been inhibited by the user
bool magCalDenied = !use_compass() || (PV_AidingMode == AID_NONE) || (frontend._magCal == 2);
// inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied);
if (inhibitMagStates && inhibitWindStates) {
stateIndexLim = 15;
} else if (inhibitWindStates) {
stateIndexLim = 21;
} else {
stateIndexLim = 23;
}
// Check for tilt convergence
float alpha = 1.0f*dtIMUavg;
float temp=tiltErrVec.length();
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
tiltAlignComplete = true;
hal.console->printf("EKF tilt alignment complete\n");
}
// once tilt has converged, align yaw using magnetic field measurements
if (tiltAlignComplete && !yawAlignComplete) {
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
StoreQuatReset();
yawAlignComplete = true;
hal.console->printf("EKF yaw alignment complete\n");
}
// store vertical position at arming to use as a reference for ground relative cehcks
if (filterArmed) {
posDownAtArming = stateStruct.position.z;
// Detect if we are about to fly so that reference readings can be taken
// Use change in motor arm status as a surrogate
if (hal.util->get_soft_armed() && !motorsArmed) {
// store vertical position at start of flight to use as a reference for ground relative checks
posDownAtTakeoff = stateStruct.position.z;
// store the range finder measurement which will be used as a reference to detect when we have taken off
rngAtStartOfFlight = rngMea;
// set the time at which we arm to assist with takeoff detection
timeAtArming_ms = imuSampleTime_ms;
}
motorsArmed = hal.util->get_soft_armed();
// Monitor the gain in height and reetthe magnetic field states and heading when initial altitude has been gained
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
if (motorsArmed && !firstMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -1.5f) {
// Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after commencement of flight
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
StoreQuatReset();
firstMagYawInit = true;
} else if (motorsArmed && !secondMagYawInit && (stateStruct.position.z - posDownAtTakeoff) < -5.0f) {
// Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after commencement of flight
// This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
StoreQuatReset();
secondMagYawInit = true;
}
// Set the type of inertial navigation aiding
setAidingMode();
}
// Detect if we are in flight
void NavEKF2_core::detectFlight()
{
// if we are a fly forward type vehicle (eg plane), then in-air mode can be determined through a combination of speed and height criteria
if (assume_zero_sideslip()) {
// Evaluate a numerical score that defines the likelihood we are in the air
float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);
bool highGndSpd = false;
bool highAirSpd = false;
bool largeHgtChange = false;
// trigger at 8 m/s airspeed
if (_ahrs->airspeed_sensor_enabled()) {
const AP_Airspeed *airspeed = _ahrs->get_airspeed();
if (airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 10.0f) {
highAirSpd = true;
}
}
// trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors
if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) {
highGndSpd = true;
}
// trigger if more than 10m away from initial height
if (fabsf(baroDataDelayed.hgt) > 10.0f) {
largeHgtChange = true;
}
// to go to in-air mode we also need enough GPS velocity to be able to calculate a reliable ground track heading and either a lerge height or airspeed change
if (onGround && highGndSpd && (highAirSpd || largeHgtChange)) {
onGround = false;
}
// if is possible we are in flight, set the time this condition was last detected
if (highGndSpd || highAirSpd || largeHgtChange) {
airborneDetectTime_ms = imuSampleTime_ms;
}
// after 5 seconds of not detecting a possible flight condition, we transition to on-ground mode
if(!onGround && ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
onGround = true;
}
// perform a yaw alignment check against GPS if exiting on-ground mode, bu tonly if we have enough ground speed
// this is done to protect against unrecoverable heading alignment errors due to compass faults
if (!onGround && prevOnGround) {
alignYawGPS();
}
}
// store current on-ground status for next time
prevOnGround = onGround;
}
// Set inertial navigation aiding mode
void NavEKF2_core::setAidingMode()
{
// Determine when to commence aiding for inertial navigation
// Save the previous status so we can detect when it has changed
prevIsAiding = isAiding;
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
bool filterIsStable = tiltAlignComplete && yawAlignComplete;
// If GPS useage has been prohiited then we use flow aiding provided optical flow data is present
bool useFlowAiding = (frontend._fusionModeGPS == 3) && optFlowDataPresent();
// Start aiding if we have a source of aiding data and the filter attitude algnment is complete
// Latch to on. Aiding can be turned off by setting both
isAiding = ((readyToUseGPS() || useFlowAiding) && filterIsStable) || isAiding;
// check to see if we are starting or stopping aiding and set states and modes as required
if (isAiding != prevIsAiding) {
// We have transitioned either into or out of aiding
// zero stored velocities used to do dead-reckoning
heldVelNE.zero();
// reset the flag that indicates takeoff for use by optical flow navigation
takeOffDetected = false;
// set various useage modes based on the condition at arming. These are then held until the filter is disarmed.
if (!filterArmed) {
PV_AidingMode = AID_NONE; // When dis-armed, we only estimate orientation & height using the constant position mode
// set various useage modes based on the condition when we start aiding. These are then held until aiding is stopped.
if (!isAiding) {
// We have ceased aiding
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
// store the current position to be used to keep reporting the last known position when disarmed
// store the current position to be used to keep reporting the last known position
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
// initialise filtered altitude used to provide a takeoff reference to current baro on disarm
// this reduces the time required for the filter to settle before the estimate can be used
// this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
meaHgtAtTakeOff = baroDataDelayed.hgt;
// reset the vertical position state to faster recover from baro errors experienced during touchdown
stateStruct.position.z = -meaHgtAtTakeOff;
} else if (frontend._fusionModeGPS == 3) { // arming when GPS useage has been prohibited
if (optFlowDataPresent()) {
} else if (frontend._fusionModeGPS == 3) {
// We have commenced aiding, but GPS useage has been prohibited so use optical flow only
hal.console->printf("EKF is using optical flow\n");
PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
posTimeout = true;
velTimeout = true;
} else {
hal.console->printf("EKF cannot use aiding\n");
PV_AidingMode = AID_NONE; // we don't have optical flow data and will only be able to estimate orientation and height
posTimeout = true;
velTimeout = true;
}
// Reset the last valid flow measurement time
flowValidMeaTime_ms = imuSampleTime_ms;
// Reset the last valid flow fusion time
prevFlowFuseTime_ms = imuSampleTime_ms;
// this avoids issues casued by the time delay associated with arming that can trigger short timeouts
rngValidMeaTime_ms = imuSampleTime_ms;
// store the range finder measurement which will be used as a reference to detect when we have taken off
rangeAtArming = rngMea;
// set the time at which we arm to assist with takeoff detection
timeAtArming_ms = imuSampleTime_ms;
} else { // arming when GPS useage is allowed
if (!gpsQualGood) {
hal.console->printf("EKF cannot use aiding\n");
PV_AidingMode = AID_NONE; // we don't have have GPS data and will only be able to estimate orientation and height
posTimeout = true;
velTimeout = true;
} else {
// We have commenced aiding and GPS useage is allowed
hal.console->printf("EKF is using GPS\n");
PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states
posTimeout = false;
@ -4565,37 +4587,19 @@ void NavEKF2_core::performArmingChecks()
// reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
lastPosPassTime_ms = imuSampleTime_ms;
}
}
// Reset all position, velocity and covariance
ResetVelocity();
ResetPosition();
CovarianceInit();
} else if (filterArmed && !firstMagYawInit && (stateStruct.position.z - posDownAtArming) < -1.5f && !assume_zero_sideslip()) {
// Do the first in-air 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)
// This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
StoreQuatReset();
firstMagYawInit = true;
} else if (filterArmed && !secondMagYawInit && (stateStruct.position.z - posDownAtArming) < -5.0f && !assume_zero_sideslip()) {
// Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
// This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
StoreQuatReset();
secondMagYawInit = true;
}
// Always turn aiding off when the vehicle is disarmed
if (!filterArmed) {
if (!isAiding) {
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
}
}
// Set the NED origin to be used until the next filter reset
@ -4621,7 +4625,7 @@ bool NavEKF2_core::getOriginLLH(struct Location &loc) const
// set the LLH location of the filters NED origin
bool NavEKF2_core::setOriginLLH(struct Location &loc)
{
if (filterArmed) {
if (isAiding) {
return false;
}
EKF_origin = loc;
@ -4811,7 +4815,7 @@ void NavEKF2_core::readRangeFinder(void)
rngMea = max(storedRngMeas[midIndex],rngOnGnd);
newDataRng = true;
rngValidMeaTime_ms = imuSampleTime_ms;
} else if (!filterArmed) {
} else if (!motorsArmed) {
// if not armed and no return, we assume on ground range
rngMea = rngOnGnd;
newDataRng = true;
@ -4826,7 +4830,7 @@ void NavEKF2_core::readRangeFinder(void)
// Detect takeoff for optical flow navigation
void NavEKF2_core::detectOptFlowTakeoff(void)
{
if (filterArmed && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
if (motorsArmed && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
const AP_InertialSensor &ins = _ahrs->get_ins();
Vector3f angRateVec;
Vector3f gyroBias;
@ -4838,7 +4842,7 @@ void NavEKF2_core::detectOptFlowTakeoff(void)
angRateVec = ins.get_gyro() - gyroBias;
}
takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rangeAtArming + 0.1f)));
takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rngAtStartOfFlight + 0.1f)));
}
}

23
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -426,9 +426,6 @@ private:
// calculate the NED earth spin vector in rad/sec
void calcEarthRateNED(Vector3f &omega, int32_t latitude) const;
// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
void SetFlightAndFusionModes();
// initialise the covariance matrix
void CovarianceInit();
@ -513,8 +510,14 @@ private:
// fuse optical flow measurements into the main filter
void FuseOptFlow();
// Check arm status and perform required checks and mode changes
void performArmingChecks();
// Control filter mode changes
void controlFilterModes();
// Determine if we are flying or on the ground
void detectFlight();
// Set inertial navigaton aiding mode
void setAidingMode();
// Set the NED origin to be used until the next filter reset
void setOrigin();
@ -646,20 +649,19 @@ private:
float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
bool firstArmComplete; // true when first transition out of static mode has been performed after start up
bool firstMagYawInit; // true when the first post takeoff initialisation of earth field and yaw angle has been performed
bool secondMagYawInit; // true when the second post takeoff initialisation of earth field and yaw angle has been performed
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 filterArmed; // true when the vehicle is disarmed
bool prevFilterArmed; // vehicleArmed from previous frame
bool isAiding; // true when the filter is fusing position, velocity or flow measurements
bool prevIsAiding; // isAiding from previous frame
struct Location EKF_origin; // LLH origin of the NED axis system - do not change unless filter is reset
bool validOrigin; // true when the EKF origin is valid
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the UBlox GPS receiver
uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
Vector3f lastMagOffsets; // magnetometer offsets returned by compass object from previous update
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
float posDownAtArming; // flight vehicle vertical position at arming used as a reference point
float posDownAtTakeoff; // flight vehicle vertical position at arming used as a reference point
bool highYawRate; // true when the vehicle is doing rapid yaw rotation where gyro scale factor errors could cause loss of heading reference
float yawRateFilt; // filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
bool useGpsVertVel; // true if GPS vertical velocity should be used
@ -698,6 +700,7 @@ private:
bool gpsQualGood; // true when the GPS quality can be used to initialise the navigation system
uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks
bool consistentMagData; // true when the magnetometers are passing consistency checks
bool motorsArmed; // true when the motors have been armed
// variables added for optical flow fusion
of_elements storedOF[OBS_BUFFER_LENGTH]; // OF data buffer
@ -756,7 +759,7 @@ private:
// Movement detector
bool takeOffDetected; // true when takeoff for optical flow navigation has been detected
float rangeAtArming; // range finder measurement when armed
float rngAtStartOfFlight; // range finder measurement at start of flight
uint32_t timeAtArming_ms; // time in msec that the vehicle armed
// baro ground effect