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

AP_NavEKF2: Fix timer wrapping bugs 

Also replaces tab characters
This commit is contained in:
Paul Riseborough 2020-04-21 10:32:39 +10:00 committed by Andrew Tridgell
parent 12d639fc17
commit 0d965d2a1c
3 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -240,8 +240,8 @@ void NavEKF2_core::setAidingMode()
} }
posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) && posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
posAidLossPending = (imuSampleTime_ms > lastRngBcnPassTime_ms + frontend->_gsfResetDelay) && posAidLossPending = (imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->_gsfResetDelay) &&
(imuSampleTime_ms > lastPosPassTime_ms + frontend->_gsfResetDelay); (imuSampleTime_ms - lastPosPassTime_ms > frontend->_gsfResetDelay);
} }
if (attAidLossCritical) { if (attAidLossCritical) {
@ -544,7 +544,7 @@ void NavEKF2_core::runYawEstimator()
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) { if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
float trueAirspeed; float trueAirspeed;
if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) { if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) {
if (imuDataDelayed.time_ms < (tasDataDelayed.time_ms + 5000)) { if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) {
trueAirspeed = tasDataDelayed.tas; trueAirspeed = tasDataDelayed.tas;
} else { } else {
trueAirspeed = defaultAirSpeed * AP::ahrs().get_EAS2TAS(); trueAirspeed = defaultAirSpeed * AP::ahrs().get_EAS2TAS();
@ -562,7 +562,7 @@ void NavEKF2_core::runYawEstimator()
} }
// action an external reset request // action an external reset request
if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms < (EKFGSF_yaw_reset_request_ms + YAW_RESET_TO_GSF_TIMEOUT_MS)) { if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms - EKFGSF_yaw_reset_request_ms < YAW_RESET_TO_GSF_TIMEOUT_MS) {
EKFGSF_resetMainFilterYaw(); EKFGSF_resetMainFilterYaw();
} }
} }

10
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -838,7 +838,7 @@ void NavEKF2_core::fuseEulerYaw()
extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z); extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z);
measured_yaw = euler321.z; measured_yaw = euler321.z;
} else { } else {
if (imuSampleTime_ms > prevBetaStep_ms + 1000 && yawEstimator != nullptr) { if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) {
float gsfYaw, gsfYawVariance; float gsfYaw, gsfYawVariance;
if (yawEstimator->getYawData(&gsfYaw, &gsfYawVariance) && if (yawEstimator->getYawData(&gsfYaw, &gsfYawVariance) &&
is_positive(gsfYawVariance) && is_positive(gsfYawVariance) &&
@ -897,7 +897,7 @@ void NavEKF2_core::fuseEulerYaw()
euler312 = extNavDataDelayed.quat.to_vector312(); euler312 = extNavDataDelayed.quat.to_vector312();
measured_yaw = euler312.z; measured_yaw = euler312.z;
} else { } else {
if (imuSampleTime_ms > prevBetaStep_ms + 1000 && yawEstimator != nullptr) { if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) {
float gsfYaw, gsfYawVariance; float gsfYaw, gsfYawVariance;
if (yawEstimator->getYawData(&gsfYaw, &gsfYawVariance) && if (yawEstimator->getYawData(&gsfYaw, &gsfYawVariance) &&
is_positive(gsfYawVariance) && is_positive(gsfYawVariance) &&
@ -1247,9 +1247,9 @@ bool NavEKF2_core::EKFGSF_resetMainFilterYaw()
ResetVelocity(); ResetVelocity();
ResetPosition(); ResetPosition();
// reset test ratios that are reported to prevent a race condition with the external state machine requesting the reset // reset test ratios that are reported to prevent a race condition with the external state machine requesting the reset
velTestRatio = 0.0f; velTestRatio = 0.0f;
posTestRatio = 0.0f; posTestRatio = 0.0f;
return true; return true;

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -1251,8 +1251,8 @@ private:
float InitialGyroBiasUncertainty(void) const; float InitialGyroBiasUncertainty(void) const;
// The following declarations are used to control when the main navigation filter resets it's yaw to the estimate provided by the GSF // The following declarations are used to control when the main navigation filter resets it's yaw to the estimate provided by the GSF
uint64_t EKFGSF_yaw_reset_ms; // timestamp of last emergency yaw reset (uSec) uint32_t EKFGSF_yaw_reset_ms; // timestamp of last emergency yaw reset (uSec)
uint64_t EKFGSF_yaw_reset_request_ms; // timestamp of last emergency yaw reset request (uSec) uint32_t EKFGSF_yaw_reset_request_ms; // timestamp of last emergency yaw reset request (uSec)
uint8_t EKFGSF_yaw_reset_count; // number of emergency yaw resets performed uint8_t EKFGSF_yaw_reset_count; // number of emergency yaw resets performed
bool EKFGSF_run_filterbank; // true when the filter bank is active bool EKFGSF_run_filterbank; // true when the filter bank is active
}; };