Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Allow copters to fly with magnetometers disabled 

Copter operation without a magnetometer is limited to constant position and relative position modes only (no GPS or range beacon fusion permitted)
Copter optical flow operation without a magnetometer is permitted.
The ability of planes to takeoff/launch without a magnetometer and align the yaw using the GPS velocity is retained.
This commit is contained in:
priseborough 2016-12-28 22:22:22 +11:00 committed by Randy Mackay
parent 2e5ac40ef6
commit a133d55b6d
3 changed files with 29 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -180,9 +180,8 @@ void NavEKF3_core::setAidingMode()
// Save the previous status so we can detect when it has changed
PV_AidingModePrev = PV_AidingMode;
bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
// Check that the gyro bias variance has converged
checkGyroCalStatus();
// Determine if we should change aiding mode
if (PV_AidingMode == AID_NONE) {
@ -190,9 +189,9 @@ void NavEKF3_core::setAidingMode()
// and IMU gyro bias estimates have stabilised
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
// GPS aiding is the preferred option unless excluded by the user
if(canUseGPS || canUseRangeBeacon) {
if(readyToUseGPS() || readyToUseRangeBeacon()) {
PV_AidingMode = AID_ABSOLUTE;
} else if (optFlowDataPresent() && filterIsStable) {
} else if (readyToUseOptFlow()) {
PV_AidingMode = AID_RELATIVE;
}
} else if (PV_AidingMode == AID_RELATIVE) {
@ -202,7 +201,7 @@ void NavEKF3_core::setAidingMode()
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// Enable switch to absolute position mode if GPS or range beacon data is available
// If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
if(canUseGPS || canUseRangeBeacon) {
if(readyToUseGPS() || readyToUseRangeBeacon()) {
PV_AidingMode = AID_ABSOLUTE;
} else if (flowSensorTimeout || flowFusionTimeout) {
PV_AidingMode = AID_NONE;
@ -305,12 +304,12 @@ void NavEKF3_core::setAidingMode()
// Reset the last valid flow fusion time
prevFlowFuseTime_ms = imuSampleTime_ms;
} else if (PV_AidingMode == AID_ABSOLUTE) {
if (canUseGPS) {
if (readyToUseGPS()) {
// We are commencing aiding using GPS - this is the preferred method
posResetSource = GPS;
velResetSource = GPS;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index);
} else if (canUseRangeBeacon) {
} else if (readyToUseRangeBeacon()) {
// We are commencing aiding using range beacons
posResetSource = RNGBCN;
velResetSource = DEFAULT;
@ -342,20 +341,14 @@ void NavEKF3_core::setAidingMode()
void NavEKF3_core::checkAttitudeAlignmentStatus()
{
// Check for tilt convergence - used during initial alignment
if (norm(P[0][0],P[1][1],P[2][2],P[3][3]) < sq(0.03f) && !tiltAlignComplete) {
if (norm(P[0][0],P[1][1],P[2][2],P[3][3]) < sq(0.035f) && !tiltAlignComplete) {
tiltAlignComplete = true;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete\n",(unsigned)imu_index);
}
// submit yaw and magnetic field reset requests depending on whether we have compass data
if (tiltAlignComplete && !yawAlignComplete) {
if (use_compass()) {
// submit yaw and magnetic field reset request depending on whether we have compass data
if (use_compass()&& !yawAlignComplete && tiltAlignComplete) {
magYawResetRequest = true;
gpsYawResetRequest = false;
} else {
magYawResetRequest = false;
gpsYawResetRequest = true;
}
}
}
@ -372,22 +365,23 @@ bool NavEKF3_core::useRngFinder(void) const
return true;
}
// return true if optical flow data is available
bool NavEKF3_core::optFlowDataPresent(void) const
// return true if the filter is ready to start using optical flow measurements
bool NavEKF3_core::readyToUseOptFlow(void) const
{
return (imuSampleTime_ms - flowMeaTime_ms < 200);
// We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use optical flow
return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned;
}
// return true if the filter to be ready to use gps
bool NavEKF3_core::readyToUseGPS(void) const
{
return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse;
return validOrigin && tiltAlignComplete && yawAlignComplete && delAngBiasLearned && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse && !gpsInhibit;
}
// return true if the filter to be ready to use the beacon range measurements
bool NavEKF3_core::readyToUseRangeBeacon(void) const
{
return tiltAlignComplete && yawAlignComplete && rngBcnGoodToAlign && rngBcnDataToFuse;
return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnGoodToAlign && rngBcnDataToFuse;
}
// return true if we should use the compass
@ -440,15 +434,14 @@ void NavEKF3_core::recordYawReset()
}
}
// return true and set the class variable true if the delta angle bias has been learned
bool NavEKF3_core::checkGyroCalStatus(void)
// set the class variable true if the delta angle bias variances are sufficiently small
void NavEKF3_core::checkGyroCalStatus(void)
{
// check delta angle bias variances
const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) &&
(P[11][11] <= delAngBiasVarMax) &&
(P[12][12] <= delAngBiasVarMax);
return delAngBiasLearned;
}
// Commands the EKF to not use GPS.
@ -477,7 +470,8 @@ void NavEKF3_core::updateFilterStatus(void)
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
bool optFlowNavPossible = flowDataValid && delAngBiasLearned;
bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE)));
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE)));
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;

9
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -274,10 +274,11 @@ void NavEKF3_core::SelectMagFusion()
}
}
// If we have no magnetometer and are on the ground, fuse in a synthetic heading measurement to prevent the
// filter covariances from becoming badly conditioned
// If we have no magnetometer, fuse in a synthetic heading measurement at 7Hz to prevent the filter covariances
// from becoming badly conditioned. For planes we only do this on-ground because they can align the yaw from GPS when
// airborne. For other platform types we do this all the time.
if (!use_compass()) {
if (onGround && (imuSampleTime_ms - lastSynthYawTime_ms > 1000)) {
if ((onGround || assume_zero_sideslip()) && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) {
fuseEulerYaw();
magTestRatio.zero();
yawTestRatio = 0.0f;
@ -807,7 +808,7 @@ void NavEKF3_core::fuseEulerYaw()
// If we can't use compass data, set the meaurement to the predicted
// to prevent uncontrolled variance growth whilst on ground without magnetometer
float measured_yaw;
if (use_compass() && yawAlignComplete && magStateInitComplete) {
if (use_compass() && yawAlignComplete) {
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + _ahrs->get_compass()->get_declination());
} else {
measured_yaw = predicted_yaw;

8
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -620,8 +620,8 @@ private:
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
void calcIMU_Weighting(float K1, float K2);
// return true if optical flow data is available
bool optFlowDataPresent(void) const;
// return true if the filter is ready to start using optical flow measurements
bool readyToUseOptFlow(void) const;
// return true if we should use the range finder sensor
bool useRngFinder(void) const;
@ -667,8 +667,8 @@ private:
// Assess GPS data quality and return true if good enough to align the EKF
bool calcGpsGoodToAlign(void);
// return true and set the class variable true if the delta angle bias has been learned
bool checkGyroCalStatus(void);
// set the class variable true if the delta angle bias variances are sufficiently small
void checkGyroCalStatus(void);
// update inflight calculaton that determines if GPS data is good enough for reliable navigation
void calcGpsGoodForFlight(void);