From 12d639fc171d8189b7de4e15954974d84efaca41 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 18 Apr 2020 20:58:00 +1000 Subject: [PATCH] AP_NavEKF3: Misc changes arising from review AP_NavEKF3: Fix typo in comment AP_NavEKF3: Use uint32_t data type for msec timers AP_NavEKF3: Misc msec timer fixes Fix wrapping bug. Use common timestamp. AP_NavEKF3: Format fixes - remove tabs AP_NavEKF3: Fix typing and wrap issue on time comparison --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 8 ++++---- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 13 ++++++------- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 4 ++-- 3 files changed, 12 insertions(+), 13 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 9ba410b9fe..af2a4471a5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -282,8 +282,8 @@ void NavEKF3_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 > (uint32_t)frontend->_gsfResetDelay) && + (imuSampleTime_ms - lastPosPassTime_ms > (uint32_t)frontend->_gsfResetDelay); } if (attAidLossCritical) { @@ -470,7 +470,7 @@ bool NavEKF3_core::use_compass(void) const bool NavEKF3_core::using_external_yaw(void) const { if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK || effectiveMagCal == MagCal::GSF_YAW) { - return AP_HAL::millis() - last_gps_yaw_fusion_ms < 5000 || AP_HAL::millis() - lastSynthYawTime_ms < 5000; + return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; } return false; } @@ -602,7 +602,7 @@ void NavEKF3_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(); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index eaae2feebe..c3ae51049a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -293,7 +293,7 @@ void NavEKF3_core::SelectMagFusion() float yawEKFGSF, yawVarianceEKFGSF; bool canUseEKFGSF = yawEstimator->getYawData(&yawEKFGSF, &yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f)); if (yawAlignComplete && canUseEKFGSF) { - // use the EKF-GSF yaw estimator output as this is more robust that the EKF can achieve without a yaw measurement + // use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement yawAngDataDelayed.yawAngErr = MAX(sqrtf(yawVarianceEKFGSF), 0.05f); yawAngDataDelayed.yawAng = yawEKFGSF; fuseEulerYaw(false, true); @@ -325,7 +325,6 @@ void NavEKF3_core::SelectMagFusion() // Handle case where we are using an external yaw sensor instead of a magnetomer if (effectiveMagCal == MagCal::EXTERNAL_YAW || effectiveMagCal == MagCal::EXTERNAL_YAW_FALLBACK) { bool have_fused_gps_yaw = false; - uint32_t now = AP_HAL::millis(); if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) { if (tiltAlignComplete && !yawAlignComplete) { alignYawAngle(); @@ -333,7 +332,7 @@ void NavEKF3_core::SelectMagFusion() fuseEulerYaw(false, true); } have_fused_gps_yaw = true; - last_gps_yaw_fusion_ms = now; + last_gps_yaw_fusion_ms = imuSampleTime_ms; } else if (tiltAlignComplete && !yawAlignComplete && (imuSampleTime_ms - lastSynthYawTime_ms > 140)) { yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f); // update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops @@ -379,7 +378,7 @@ void NavEKF3_core::SelectMagFusion() // we don't have GPS yaw data and are configured for // fallback. If we've only just lost GPS yaw - if (now - last_gps_yaw_fusion_ms < 10000) { + if (imuSampleTime_ms - last_gps_yaw_fusion_ms < 10000) { // don't fallback to magnetometer fusion for 10s return; } @@ -1525,9 +1524,9 @@ bool NavEKF3_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; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 6a7a8c2bc4..07a286279d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1394,8 +1394,8 @@ private: void updateMovementCheck(void); // 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 };