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

AP_NavEKF2: Fix timer wrapping bugs

Browse Source 
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) &&
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
posAidLossPending = (imuSampleTime_ms > lastRngBcnPassTime_ms + frontend->_gsfResetDelay) &&
(imuSampleTime_ms > lastPosPassTime_ms + frontend->_gsfResetDelay);
posAidLossPending = (imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->_gsfResetDelay) &&
(imuSampleTime_ms - lastPosPassTime_ms > frontend->_gsfResetDelay);
}
if (attAidLossCritical) {
@ -544,7 +544,7 @@ void NavEKF2_core::runYawEstimator()
if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
float trueAirspeed;
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;
} else {
trueAirspeed = defaultAirSpeed * AP::ahrs().get_EAS2TAS();
@ -562,7 +562,7 @@ void NavEKF2_core::runYawEstimator()
}
// 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();
}
}

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

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -1251,8 +1251,8 @@ private:
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
uint64_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_ms; // timestamp of last emergency yaw reset (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
bool EKFGSF_run_filterbank; // true when the filter bank is active
};