zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF2: Fix reporting of pre-flight GPS checks 

Re-order checks so that that less important messages are not hidden when the 40 character buffer overflows
Add required output methods
master
Paul Riseborough 9 years ago committed by Andrew Tridgell
parent
7bd61a484c
commit
b6d63d4b4e
5 changed files with 112 additions and 83 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 9
      libraries/AP_NavEKF2/AP_NavEKF2.cpp
  2. 3
      libraries/AP_NavEKF2/AP_NavEKF2.h
  3. 11
      libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
  4. 169
      libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
  5. 3
      libraries/AP_NavEKF2/AP_NavEKF2_core.h

9
libraries/AP_NavEKF2/AP_NavEKF2.cpp
Unescape View File

@ -837,4 +837,13 @@ uint32_t NavEKF2::getLastVelNorthEastReset(Vector2f &vel) const @@ -837,4 +837,13 @@ uint32_t NavEKF2::getLastVelNorthEastReset(Vector2f &vel) const
return core->getLastVelNorthEastReset(vel);
}
// report the reason for why the backend is refusing to initialise
const char *NavEKF2::prearm_failure_reason(void) const
{
if (!core) {
return nullptr;
}
return core->prearm_failure_reason();
}
#endif //HAL_CPU_CLASS

3
libraries/AP_NavEKF2/AP_NavEKF2.h
Unescape View File

@ -235,6 +235,9 @@ public: @@ -235,6 +235,9 @@ public:
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastVelNorthEastReset(Vector2f &vel) const;
// report any reason for why the backend is refusing to initialise
const char *prearm_failure_reason(void) const;
// allow the enable flag to be set by Replay
void set_enable(bool enable) { _enable.set(enable); }

11
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
Unescape View File

@ -505,4 +505,15 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan) @@ -505,4 +505,15 @@ void NavEKF2_core::send_status_report(mavlink_channel_t chan)
}
// report the reason for why the backend is refusing to initialise
const char *NavEKF2_core::prearm_failure_reason(void) const
{
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
// we are not failing
return nullptr;
}
return prearm_fail_string;
}
#endif // HAL_CPU_CLASS

169
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
Unescape View File

@ -23,62 +23,6 @@ extern const AP_HAL::HAL& hal; @@ -23,62 +23,6 @@ extern const AP_HAL::HAL& hal;
*/
bool NavEKF2_core::calcGpsGoodToAlign(void)
{
// fail if reported speed accuracy greater than threshold
bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f) && (frontend._gpsCheck & MASK_GPS_SPD_ERR);
// Report check result as a text string and bitmask
if (gpsSpdAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS speed error %.1f", (double)gpsSpdAccuracy);
gpsCheckStatus.bad_sAcc = true;
} else {
gpsCheckStatus.bad_sAcc = false;
}
// fail if not enough sats
bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend._gpsCheck & MASK_GPS_NSATS);
// Report check result as a text string and bitmask
if (numSatsFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats());
gpsCheckStatus.bad_sats = true;
} else {
gpsCheckStatus.bad_sats = false;
}
// fail if satellite geometry is poor
bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend._gpsCheck & MASK_GPS_HDOP);
// Report check result as a text string and bitmask
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()));
gpsCheckStatus.bad_hdop = true;
} else {
gpsCheckStatus.bad_hdop = false;
}
// fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f;
bool hAccFail;
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
hAccFail = (hAcc > 5.0f) && (frontend._gpsCheck & MASK_GPS_POS_ERR);
} else {
hAccFail = false;
}
// Report check result as a text string and bitmask
if (hAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS horiz error %.1f", (double)hAcc);
gpsCheckStatus.bad_hAcc = true;
} else {
gpsCheckStatus.bad_hAcc = false;
}
// If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states
// This enables us to handle large changes to the external magnetic field environment that occur before arming
if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f && yawTestRatio <= 1.0f) || !consistentMagData) {
@ -93,28 +37,6 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) @@ -93,28 +37,6 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
magYawResetTimer_ms = imuSampleTime_ms;
}
// 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 || yawTestRatio > 1.0f) && (frontend._gpsCheck & MASK_GPS_YAW_ERR)) {
yawFail = true;
} else {
yawFail = false;
}
// Report check result as a text string and bitmask
if (yawFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"Mag yaw error x=%.1f y=%.1f yaw=%.1f",
(double)magTestRatio.x,
(double)magTestRatio.y,
(double)yawTestRatio);
gpsCheckStatus.bad_yaw = true;
} else {
gpsCheckStatus.bad_yaw = false;
}
// Check for significant change in GPS position if disarmed which indicates bad GPS
// This check can only be used when the vehicle is stationary
const struct Location &gpsloc = _ahrs->get_gps().location(); // Current location
@ -188,18 +110,99 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) @@ -188,18 +110,99 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
gpsCheckStatus.bad_horiz_vel = false;
}
// record time of fail - assume fail first time called
if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail || lastGpsVelFail_ms == 0) {
// fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f;
bool hAccFail;
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
hAccFail = (hAcc > 5.0f) && (frontend._gpsCheck & MASK_GPS_POS_ERR);
} else {
hAccFail = false;
}
// Report check result as a text string and bitmask
if (hAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS horiz error %.1f", (double)hAcc);
gpsCheckStatus.bad_hAcc = true;
} else {
gpsCheckStatus.bad_hAcc = false;
}
// fail if reported speed accuracy greater than threshold
bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f) && (frontend._gpsCheck & MASK_GPS_SPD_ERR);
// Report check result as a text string and bitmask
if (gpsSpdAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS speed error %.1f", (double)gpsSpdAccuracy);
gpsCheckStatus.bad_sAcc = true;
} else {
gpsCheckStatus.bad_sAcc = false;
}
// fail if satellite geometry is poor
bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend._gpsCheck & MASK_GPS_HDOP);
// Report check result as a text string and bitmask
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()));
gpsCheckStatus.bad_hdop = true;
} else {
gpsCheckStatus.bad_hdop = false;
}
// fail if not enough sats
bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend._gpsCheck & MASK_GPS_NSATS);
// Report check result as a text string and bitmask
if (numSatsFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats());
gpsCheckStatus.bad_sats = true;
} else {
gpsCheckStatus.bad_sats = false;
}
// 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) && (frontend._gpsCheck & MASK_GPS_YAW_ERR)) {
yawFail = true;
} else {
yawFail = false;
}
// Report check result as a text string and bitmask
if (yawFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"Mag yaw error x=%.1f y=%.1f",
(double)magTestRatio.x,
(double)magTestRatio.y);
gpsCheckStatus.bad_yaw = true;
} else {
gpsCheckStatus.bad_yaw = false;
}
// assume failed first time through and notify user checks have started
if (lastGpsVelFail_ms == 0) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks");
lastGpsVelFail_ms = imuSampleTime_ms;
}
// record time of fail
if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) {
lastGpsVelFail_ms = imuSampleTime_ms;
}
// continuous period without fail required to return a healthy status
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
return true;
} else {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF checking GPS");
return false;
}
return false;
}
// update inflight calculaton that determines if GPS data is good enough for reliable navigation

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h
Unescape View File

@ -256,6 +256,9 @@ public: @@ -256,6 +256,9 @@ public:
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastVelNorthEastReset(Vector2f &vel) const;
// report any reason for why the backend is refusing to initialise
const char *prearm_failure_reason(void) const;
private:
// Reference to the global EKF frontend for parameters
NavEKF2 &frontend;

Write Preview
Loading…
Cancel
Save