aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Allow user to select preflight GPS checks

Browse Source
This commit is contained in:
Paul Riseborough 2015-09-22 23:13:16 +10:00 committed by Randy Mackay
parent cdae84aec1
commit 21e4910149
2 changed files with 24 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -384,6 +384,12 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("ALT_SOURCE", 32, NavEKF, _altSource, 1),
// @Param: GPS_CHECK
// @DisplayName: GPS preflight check
// @Description: 1 byte bitmap of GPS preflight checks to perform. Set to 0 to bypass all checks. Set to 255 perform all checks. Set to 1 to check just the number of satellites and HDoP. Set to 10 for the best checking that will still allow checks to pass when the copter is moving, eg launch from a boat.
// @Bitmask: 0:NSats,1:HDoP,2:speed error,3:horiz pos error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed
// @User: Advanced
AP_GROUPINFO("GPS_CHECK", 33, NavEKF, _gpsCheck, 10),
AP_GROUPEND
};
@ -5211,7 +5217,7 @@ bool NavEKF::calcGpsGoodToAlign(void)
}
// fail if velocity difference or reported speed accuracy greater than threshold
bool gpsVelFail = (velDiffAbs > 1.0f) || (gpsSpdAccuracy > 1.0f);
bool gpsVelFail = ((velDiffAbs > 1.0f) || (gpsSpdAccuracy > 1.0f)) && (_gpsCheck & MASK_GPS_SPD_ERR);
if (velDiffAbs > 1.0f) {
hal.util->snprintf(prearm_fail_string,
@ -5225,14 +5231,14 @@ bool NavEKF::calcGpsGoodToAlign(void)
}
// fail if not enough sats
bool numSatsFail = _ahrs->get_gps().num_sats() < 6;
bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (_gpsCheck & MASK_GPS_NSATS);
if (numSatsFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats());
}
// fail if satellite geometry is poor
bool hdopFail = _ahrs->get_gps().get_hdop() > 250;
bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (_gpsCheck & MASK_GPS_HDOP);
if (hdopFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS HDOP %.1f (needs 2.5)", (double)(0.01f * _ahrs->get_gps().get_hdop()));
@ -5242,7 +5248,7 @@ bool NavEKF::calcGpsGoodToAlign(void)
float hAcc = 0.0f;
bool hAccFail;
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
hAccFail = hAcc > 5.0f;
hAccFail = (hAcc > 5.0f) && (_gpsCheck & MASK_GPS_POS_ERR);
} else {
hAccFail = false;
}
@ -5269,7 +5275,7 @@ bool NavEKF::calcGpsGoodToAlign(void)
// fail if magnetometer innovations are outside limits indicating bad yaw
// with bad yaw we are unable to use GPS
bool yawFail;
if (magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) {
if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) && (_gpsCheck & MASK_GPS_YAW_ERR)) {
yawFail = true;
} else {
yawFail = false;
@ -5297,7 +5303,7 @@ bool NavEKF::calcGpsGoodToAlign(void)
gpsDriftNE = min(gpsDriftNE,10.0f);
// Fail if more than 3 metres drift after filtering whilst pre-armed when the vehicle is supposed to be stationary
// This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m
bool gpsDriftFail = gpsDriftNE > 3.0f && !vehicleArmed;
bool gpsDriftFail = (gpsDriftNE > 3.0f) && !vehicleArmed && (_gpsCheck & MASK_GPS_POS_DRIFT);
if (gpsDriftFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
@ -5310,7 +5316,7 @@ bool NavEKF::calcGpsGoodToAlign(void)
// check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * velNED.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f) && (_gpsCheck & MASK_GPS_VERT_SPD);
} else if ((_fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) {
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
gpsVertVelFail = true;
@ -5328,7 +5334,7 @@ bool NavEKF::calcGpsGoodToAlign(void)
if (!vehicleArmed) {
gpsHorizVelFilt = 0.1f * pythagorous2(velNED.x,velNED.y) + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f);
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f) && (_gpsCheck & MASK_GPS_HORIZ_SPD);
} else {
gpsHorizVelFail = false;
}

10
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -39,6 +39,15 @@
#include <systemlib/perf_counter.h>
#endif
// GPS pre-flight check bit locations
#define MASK_GPS_NSATS (1<<0)
#define MASK_GPS_HDOP (1<<1)
#define MASK_GPS_SPD_ERR (1<<2)
#define MASK_GPS_POS_ERR (1<<3)
#define MASK_GPS_YAW_ERR (1<<4)
#define MASK_GPS_POS_DRIFT (1<<5)
#define MASK_GPS_VERT_SPD (1<<6)
#define MASK_GPS_HORIZ_SPD (1<<7)
class AP_AHRS;
@ -503,6 +512,7 @@ private:
AP_Float _maxFlowRate; // Maximum flow rate magnitude that will be accepted by the filter
AP_Int8 _fallback; // EKF-to-DCM fallback strictness. 0 = trust EKF more, 1 = fallback more conservatively.
AP_Int8 _altSource; // Primary alt source during optical flow navigation. 0 = use Baro, 1 = use range finder.
AP_Int8 _gpsCheck; // Bitmask controlling which preflight GPS checks are bypassed
// Tuning parameters
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration