|
|
|
@ -79,7 +79,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
@@ -79,7 +79,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|
|
|
|
if (gpsDriftFail) { |
|
|
|
|
hal.util->snprintf(prearm_fail_string, |
|
|
|
|
sizeof(prearm_fail_string), |
|
|
|
|
"GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler)); |
|
|
|
|
"GNSS定位飘移 %.1fm (需小于 %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler)); |
|
|
|
|
// "GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler));
|
|
|
|
|
gpsCheckStatus.bad_horiz_drift = true; |
|
|
|
|
} else { |
|
|
|
|
gpsCheckStatus.bad_horiz_drift = false; |
|
|
|
@ -110,7 +111,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
@@ -110,7 +111,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|
|
|
|
if (gpsVertVelFail) { |
|
|
|
|
hal.util->snprintf(prearm_fail_string, |
|
|
|
|
sizeof(prearm_fail_string), |
|
|
|
|
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler)); |
|
|
|
|
"GNSS垂直速度过大 %.2fm/s (需小于 %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler)); |
|
|
|
|
// "GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler));
|
|
|
|
|
gpsCheckStatus.bad_vert_vel = true; |
|
|
|
|
} else { |
|
|
|
|
gpsCheckStatus.bad_vert_vel = false; |
|
|
|
@ -131,7 +133,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
@@ -131,7 +133,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|
|
|
|
if (gpsHorizVelFail) { |
|
|
|
|
hal.util->snprintf(prearm_fail_string, |
|
|
|
|
sizeof(prearm_fail_string), |
|
|
|
|
"GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler)); |
|
|
|
|
"GNSS水平速度过大 %.2fm/s (需小于 %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler)); |
|
|
|
|
// "GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler));
|
|
|
|
|
gpsCheckStatus.bad_horiz_vel = true; |
|
|
|
|
} else { |
|
|
|
|
gpsCheckStatus.bad_horiz_vel = false; |
|
|
|
@ -150,7 +153,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
@@ -150,7 +153,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|
|
|
|
if (hAccFail) { |
|
|
|
|
hal.util->snprintf(prearm_fail_string, |
|
|
|
|
sizeof(prearm_fail_string), |
|
|
|
|
"GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler)); |
|
|
|
|
"GNSS水平定位差值过大 %.1fm (需小于 %.1f)", (double)hAcc, (double)(5.0f*checkScaler)); |
|
|
|
|
// "GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler));
|
|
|
|
|
gpsCheckStatus.bad_hAcc = true; |
|
|
|
|
} else { |
|
|
|
|
gpsCheckStatus.bad_hAcc = false; |
|
|
|
@ -166,7 +170,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
@@ -166,7 +170,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|
|
|
|
if (vAccFail) { |
|
|
|
|
hal.util->snprintf(prearm_fail_string, |
|
|
|
|
sizeof(prearm_fail_string), |
|
|
|
|
"GPS vert error %.1fm (needs < %.1f)", (double)vAcc, (double)(7.5f * checkScaler)); |
|
|
|
|
"GNSS垂直定位差值过大 %.1fm (需小于 < %.1f)", (double)vAcc, (double)(7.5f * checkScaler)); |
|
|
|
|
// "GPS vert error %.1fm (needs < %.1f)", (double)vAcc, (double)(7.5f * checkScaler));
|
|
|
|
|
gpsCheckStatus.bad_vAcc = true; |
|
|
|
|
} else { |
|
|
|
|
gpsCheckStatus.bad_vAcc = false; |
|
|
|
@ -179,7 +184,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
@@ -179,7 +184,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|
|
|
|
if (gpsSpdAccFail) { |
|
|
|
|
hal.util->snprintf(prearm_fail_string, |
|
|
|
|
sizeof(prearm_fail_string), |
|
|
|
|
"GPS speed error %.1f (needs < %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler)); |
|
|
|
|
"GNSS速度差值过大 %.1f (需小于 < %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler)); |
|
|
|
|
// "GPS speed error %.1f (needs < %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler));
|
|
|
|
|
gpsCheckStatus.bad_sAcc = true; |
|
|
|
|
} else { |
|
|
|
|
gpsCheckStatus.bad_sAcc = false; |
|
|
|
@ -191,19 +197,21 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
@@ -191,19 +197,21 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|
|
|
|
// 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 * gps.get_hdop())); |
|
|
|
|
"GNSS HDOP %.1f (需小于 2.5)", (double)(0.01f * gps.get_hdop())); |
|
|
|
|
// "GPS HDOP %.1f (needs 2.5)", (double)(0.01f * gps.get_hdop()));
|
|
|
|
|
gpsCheckStatus.bad_hdop = true; |
|
|
|
|
} else { |
|
|
|
|
gpsCheckStatus.bad_hdop = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// fail if not enough sats
|
|
|
|
|
bool numSatsFail = (gps.num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS); |
|
|
|
|
bool numSatsFail = (gps.num_sats() < 9) && (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)", gps.num_sats()); |
|
|
|
|
"GNSS卫星数 %u (需大于 9)", gps.num_sats()); |
|
|
|
|
// "GPS numsats %u (needs 6)", gps.num_sats());
|
|
|
|
|
gpsCheckStatus.bad_sats = true; |
|
|
|
|
} else { |
|
|
|
|
gpsCheckStatus.bad_sats = false; |
|
|
|
@ -232,7 +240,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
@@ -232,7 +240,8 @@ void NavEKF2_core::calcGpsGoodToAlign(void)
|
|
|
|
|
|
|
|
|
|
// 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"); |
|
|
|
|
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF开始GNSS检查"); |
|
|
|
|
// hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks");
|
|
|
|
|
lastGpsVelFail_ms = imuSampleTime_ms; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|