diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 6e337083ff01b8665131dd52203a904d29cbfdc5..71808ccc21f640efd13d660f6df6e407b05d1161 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -325,7 +325,9 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
prevStaticMode(true), // staticMode 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
+ inhibitMagStates(true), // inhibit magnetometer state updates on startup
+ onGround(true), // initialise assuming we are on ground
+ manoeuvring(false) // initialise assuming we are not maneouvring
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
,_perf_UpdateFilter(perf_alloc(PC_ELAPSED, "EKF_UpdateFilter")),
@@ -495,7 +497,7 @@ void NavEKF::InitialiseFilterDynamic(void)
ResetVelocity();
ResetPosition();
ResetHeight();
- state.body_magfield = magBias;
+ state.body_magfield.zero();
// set to true now that states have be initialised
statesInitialised = true;
@@ -568,7 +570,7 @@ void NavEKF::InitialiseFilterBootstrap(void)
state.accel_zbias1 = 0;
state.accel_zbias2 = 0;
state.wind_vel.zero();
- state.body_magfield = magBias;
+ state.body_magfield.zero();
// set to true now we have intialised the states
statesInitialised = true;
@@ -628,9 +630,6 @@ void NavEKF::UpdateFilter()
ResetPosition();
ResetHeight();
StoreStatesReset();
- // clear the magnetometer failed status as the failure may have been
- // caused by external field disturbances associated with pre-flight activities
- magFailed = false;
calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
prevStaticMode = staticMode;
}
@@ -751,51 +750,42 @@ void NavEKF::SelectVelPosFusion()
// select fusion of magnetometer data
void NavEKF::SelectMagFusion()
{
- if(!magFailed) {
- // check for and read new magnetometer measurements
- readMagData();
+ // check for and read new magnetometer measurements
+ readMagData();
- // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
- // If we have a vehicle that can fly without a compass (a vehicle that doesn't have significant sideslip) then the compass is permanently failed and will not be used until the filter is reset
- if (magHealth) {
- lastHealthyMagTime_ms = imuSampleTime_ms;
- } else {
- if ((imuSampleTime_ms - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) {
- magTimeout = true;
- if (assume_zero_sideslip()) {
- magFailed = true;
- }
- } else {
- magTimeout = false;
- }
- }
+ // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
+ if (magHealth) {
+ magTimeout = false;
+ lastHealthyMagTime_ms = imuSampleTime_ms;
+ } else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > _magFailTimeLimit_ms && use_compass()) {
+ magTimeout = true;
+ }
- // determine if conditions are right to start a new fusion cycle
- bool dataReady = statesInitialised && use_compass() && newDataMag;
- if (dataReady)
- {
- fuseMagData = true;
- // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
- memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
- magUpdateCount = 0;
- }
- else
- {
- fuseMagData = false;
- }
+ // determine if conditions are right to start a new fusion cycle
+ bool dataReady = statesInitialised && use_compass() && newDataMag;
+ if (dataReady)
+ {
+ fuseMagData = true;
+ // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
+ memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
+ magUpdateCount = 0;
+ }
+ else
+ {
+ fuseMagData = false;
+ }
- // call the function that performs fusion of magnetometer data
- FuseMagnetometer();
+ // call the function that performs fusion of magnetometer data
+ FuseMagnetometer();
- // Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
- if (magUpdateCount < magUpdateCountMax) {
- magUpdateCount ++;
- for (uint8_t i = 0; i <= 9; i++) {
- states[i] += magIncrStateDelta[i];
- }
+ // Fuse corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output
+ if (magUpdateCount < magUpdateCountMax) {
+ magUpdateCount ++;
+ for (uint8_t i = 0; i <= 9; i++) {
+ states[i] += magIncrStateDelta[i];
}
-
}
+
}
// select fusion of true airspeed measurements
@@ -2878,38 +2868,55 @@ bool NavEKF::getLLH(struct Location &loc) const
// calculate whether the flight vehicle is on the ground or flying from height, airspeed and GPS speed
void NavEKF::SetFlightAndFusionModes()
{
- const AP_Airspeed *airspeed = _ahrs->get_airspeed();
- uint8_t highAirSpd = (airspeed && airspeed->use() && airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 8.0f);
- float gndSpdSq = sq(velNED[0]) + sq(velNED[1]);
- uint8_t highGndSpdStage1 = (uint8_t)(gndSpdSq > 9.0f);
- uint8_t highGndSpdStage2 = (uint8_t)(gndSpdSq > 36.0f);
- uint8_t highGndSpdStage3 = (uint8_t)(gndSpdSq > 81.0f);
- uint8_t largeHgt = (uint8_t)(fabsf(hgtMea) > 15.0f);
- uint8_t inAirSum = highAirSpd + highGndSpdStage1 + highGndSpdStage2 + highGndSpdStage3 + largeHgt;
- // if magnetometer calibration mode 1 is selected, use a manoeuvre threshold test
- // otherwise use a height and velocity test
- if ((_magCal == 1) && (accNavMagHoriz > 0.5f) && !static_mode_demanded() && use_compass()) {
- onGround = false;
+ // determine if the vehicle is manoevring
+ if (accNavMagHoriz > 0.5f){
+ manoeuvring = true;
} else {
- // detect on-ground to in-air transition
- // if we are already on the ground then 3 or more out of 5 criteria are required
- // if we are in the air then only 2 or more are required
- // this prevents rapid tansitions
- if ((onGround && (inAirSum >= 3)) || (!onGround && (inAirSum >= 2))) {
+ 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
+ uint8_t highAirSpd = 0;
+ uint8_t heightVarying = 0;
+ uint8_t highGndSpdStage1 = 0;
+ uint8_t highGndSpdStage2 = 0;
+ uint8_t highGndSpdStage3 = 0;
+ uint8_t largeHgt = 0;
+ uint8_t accelerating = 0;
+ const AP_Airspeed *airspeed = _ahrs->get_airspeed();
+ if (airspeed && airspeed->use() && airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 8.0f) highAirSpd = 1;
+ float gndSpdSq = sq(velNED[0]) + sq(velNED[1]);
+ if (fabsf(_baro.get_climb_rate()) > 0.5f) heightVarying = 1; // this will trigger during change in baro height
+ if (gndSpdSq > 9.0f) highGndSpdStage1 = 1; // trigger at 3 m/s GPS velocity
+ if (gndSpdSq > 36.0f) highGndSpdStage2 = 1; // trigger at 6 m/s GPS velocity
+ if (gndSpdSq > 81.0f) highGndSpdStage3 = 1; // trigger at 9 m/s GPS velocity
+ if (fabsf(hgtMea) > 15.0f) largeHgt = 1; // trigger if more than 15m away from initial height
+ if (accNavMag > 0.5f) accelerating = 1; // this will trigger due to air turbulence during flight
+ uint8_t inAirSum = highAirSpd + highGndSpdStage1 + highGndSpdStage2 + highGndSpdStage3 + largeHgt + heightVarying + accelerating;
+ // if we on-ground then 4 or more out of 7 criteria are required to transition to the
+ // in-air mode and we also need enough GPS velocity to be able to calculate a reliable ground track heading
+ if (onGround && (inAirSum >= 4) && highGndSpdStage2) {
onGround = false;
- } else {
+ }
+ // if is possible we are in flight, set the time this condition was last detected
+ if (inAirSum >= 1) {
+ 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;
}
- // force a yaw alignment if exiting onGround without a compass or if compass is timed out and we are a fly forward vehicle
- if (!onGround && prevOnGround && (!use_compass() || (magTimeout && assume_zero_sideslip()))) {
+ // perform a yaw alignment check against GPS if exiting on-ground mode
+ // this is done to protect against unrecoverable heading alignment errors due to compass faults
+ if (!onGround && prevOnGround) {
alignYawGPS();
}
- // If we are flying a fly-forward type vehicle without an airspeed sensor and exiting onGround
- // we set the wind velocity to the reciprocal of the velocity vector and scale states so that the
- // wind speed is equal to the 6m/s. This prevents gains being too high at the start
- // of flight if launching into a headwind until the first turn when the EKF can form a wind speed
- // estimate
- if (!onGround && prevOnGround && !useAirspeed() && assume_zero_sideslip()) {
+ // If we aren't using an airspeed sensor we set the wind velocity to the reciprocal
+ // of the velocity vector and scale states so that the wind speed is equal to 3m/s. This helps prevent gains
+ // being too high at the start of flight if launching into a headwind until the first turn when the EKF can form
+ // a wind speed estimate and also corrects bad initial wind estimates due to heading errors
+ if (!onGround && prevOnGround && !useAirspeed()) {
setWindVelStates();
}
}
@@ -2917,8 +2924,12 @@ void NavEKF::SetFlightAndFusionModes()
prevOnGround = onGround;
// If we are on ground, or in static mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || staticMode);
- // If magnetometer calibration mode is turned off by the user or we are on ground or in static mode, then inhibit magnetometer states
- inhibitMagStates = ((_magCal == 2) || !use_compass() || onGround || staticMode);
+ // request mag calibration for both in-air and manoeuvre threshold options
+ bool magCalRequested = ((_magCal == 0) && !onGround) || ((_magCal == 1) && manoeuvring);
+ // deny mag calibration request if we aren't using the compass, are in the pre-arm static mode or it has been inhibited by the user
+ bool magCalDenied = (!use_compass() || staticMode || (_magCal == 2));
+ // inhibit the magnetic field calibration if not requested or denied
+ inhibitMagStates = (!magCalRequested || magCalDenied);
}
// initialise the covariance matrix
@@ -3153,11 +3164,9 @@ void NavEKF::readMagData()
// store time of last measurement update
lastMagUpdate = _ahrs->get_compass()->last_update;
- // read compass data and assign to bias and uncorrected measurement
- // body fixed magnetic bias is opposite sign to APM compass offsets
- // we scale compass data to improve numerical conditioning
- magBias = -_ahrs->get_compass()->get_offsets() * 0.001f;
- magData = _ahrs->get_compass()->get_field() * 0.001f + magBias;
+ // Read compass data
+ // We scale compass data to improve numerical conditioning
+ magData = _ahrs->get_compass()->get_field() * 0.001f;
// get states stored at time closest to measurement time after allowance for measurement delay
RecallStates(statesAtMagMeasTime, (imuSampleTime_ms - _msecMagDelay));
@@ -3199,7 +3208,7 @@ void NavEKF::calcEarthRateNED(Vector3f &omega, int32_t latitude) const
// initialise the earth magnetic field states using declination, suppled roll/pitch
// and magnetometer measurements and return initial attitude quaternion
-// if no magnetometer data, do not update amgentic field states and assume zero yaw angle
+// if no magnetometer data, do not update magnetic field states and assume zero yaw angle
Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
{
// declare local variables required to calculate initial orientation and magnetic field
@@ -3216,7 +3225,7 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
readMagData();
// rotate the magnetic field into NED axes
- initMagNED = Tbn*(magData - magBias);
+ initMagNED = Tbn * magData;
// calculate heading of mag field rel to body heading
float magHeading = atan2f(initMagNED.y, initMagNED.x);
@@ -3225,19 +3234,27 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
// calculate yaw angle rel to true north
- yaw = magDecAng - magHeading;
- yawAligned = true;
-
- // calculate initial filter quaternion states
- initQuat.from_euler(roll, pitch, yaw);
+ if (!badMag) {
+ // if mag healthy use declination and mag measurements to calculate yaw
+ yaw = magDecAng - magHeading;
+ yawAligned = true;
+ // calculate initial filter quaternion states
+ initQuat.from_euler(roll, pitch, yaw);
+ } else {
+ // if mag failed keep current yaw
+ initQuat = state.quat;
+ }
// calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states
initQuat.rotation_matrix(Tbn);
- initMagNED = Tbn * (magData - magBias);
+ initMagNED = Tbn* magData;
// write to earth magnetic field state vector
state.earth_magfield = initMagNED;
+
+ // clear bad magnetometer status
+ badMag = false;
} else {
initQuat.from_euler(roll, pitch, 0.0f);
yawAligned = false;
@@ -3247,11 +3264,11 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
return initQuat;
}
-// this function is used to do a forced alignment of the yaw angle to aligwith the horizontal velocity
-// vector from GPS. It is used to align the yaw angle after launch or takeoff without a magnetometer.
+// this function is used to do a forced alignment of the yaw angle to align with the horizontal velocity
+// vector from GPS. It is used to align the yaw angle after launch or takeoff.
void NavEKF::alignYawGPS()
{
- if ((sq(velNED[0]) + sq(velNED[1])) > 16.0f) {
+ if ((sq(velNED[0]) + sq(velNED[1])) > 25.0f) {
float roll;
float pitch;
float oldYaw;
@@ -3259,21 +3276,26 @@ void NavEKF::alignYawGPS()
float yawErr;
// get quaternion from existing filter states and calculate roll, pitch and yaw angles
state.quat.to_euler(roll, pitch, oldYaw);
+ // calculate course yaw angle
+ oldYaw = atan2f(state.velocity.y,state.velocity.x);
// calculate yaw angle from GPS velocity
newYaw = atan2f(velNED[1],velNED[0]);
- // modify yaw angle using GPS ground course if more than 45 degrees away or if not previously aligned
- yawErr = fabsf(newYaw - oldYaw);
- if (((yawErr > 0.7854f) && (yawErr < 5.4978f)) || !yawAligned) {
+ // estimate the yaw error
+ yawErr = wrap_PI(newYaw - oldYaw);
+ // If the inertial course angle disagrees with the GPS by more than 45 degrees, we declare the compass as bad
+ badMag = (fabsf(yawErr) > 0.7854f);
+ // correct yaw angle using GPS ground course compass failed or if not previously aligned
+ if (badMag || !yawAligned) {
+ // correct the yaw angle
+ newYaw = oldYaw + yawErr;
// calculate new filter quaternion states from Euler angles
state.quat.from_euler(roll, pitch, newYaw);
// the yaw angle is now aligned so update its status
yawAligned = true;
- // set the velocity states
- if (_fusionModeGPS < 2) {
- state.velocity.x = velNED.x;
- state.velocity.y = velNED.y;
- }
- // reinitialise the quaternion, velocity and position covariances
+ // reset the position and velocity states
+ ResetPosition();
+ ResetVelocity();
+ // reset the covariance for the quaternion, velocity and position states
// zero the matrix entries
zeroRows(P,0,9);
zeroCols(P,0,9);
@@ -3291,6 +3313,10 @@ void NavEKF::alignYawGPS()
P[8][8] = P[7][7];
P[9][9] = sq(5.0f);
}
+ // Update magnetic field states if the magnetometer is bad
+ if (badMag) {
+ calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
+ }
}
}
@@ -3372,13 +3398,14 @@ void NavEKF::ZeroVariables()
lastFixTime_ms = imuSampleTime_ms;
secondLastFixTime_ms = imuSampleTime_ms;
lastDecayTime_ms = imuSampleTime_ms;
+ airborneDetectTime_ms = imuSampleTime_ms;
gpsNoiseScaler = 1.0f;
velTimeout = false;
posTimeout = false;
hgtTimeout = false;
magTimeout = false;
- magFailed = false;
+ badMag = false;
storeIndex = 0;
dtIMU = 0;
dt = 0;
@@ -3427,7 +3454,7 @@ bool NavEKF::static_mode_demanded(void) const
// return true if we should use the compass
bool NavEKF::use_compass(void) const
{
- return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw() && !magFailed;
+ return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw();
}
// decay GPS horizontal position offset to close to zero at a rate of 1 m/s
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index de48e7c9ec5b04a6b542653e1c1d0e279edb09d8..d69f98b0879468268803fe6b9c00695db7857207 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -364,7 +364,7 @@ private:
bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out
bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out
bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out
- bool magFailed; // boolean true if the magnetometer has failed
+ bool badMag; // boolean true if the magnetometer is declared to be producing bad data
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Vector31 Kfusion; // Kalman gain vector
@@ -393,6 +393,8 @@ private:
ftype hgtRate; // state for rate of change of height filter
bool onGround; // boolean true when the flight vehicle is on the ground (not flying)
bool prevOnGround; // value of onGround from previous update
+ bool manoeuvring; // boolean true when the flight vehicle is performing horizontal changes in velocity
+ uint32_t airborneDetectTime_ms; // last time flight movement was detected
Vector6 innovVelPos; // innovation output for a group of measurements
Vector6 varInnovVelPos; // innovation variance output for a group of measurements
bool fuseVelData; // this boolean causes the velNED measurements to be fused
@@ -414,7 +416,6 @@ private:
bool fuseVtasData; // boolean true when airspeed data is to be fused
float VtasMeas; // true airspeed measurement (m/s)
state_elements statesAtVtasMeasTime; // filter states at the effective measurement time
- Vector3f magBias; // magnetometer bias vector in XYZ body axes
const ftype covTimeStepMax; // maximum time allowed between covariance predictions
const ftype covDelAngMax; // maximum delta angle between covariance predictions
bool covPredStep; // boolean set to true when a covariance prediction step has been performed