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

AP_NavEKF3: Misc changes arising from review

Browse Source 
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
This commit is contained in:
Paul Riseborough 2020-04-18 20:58:00 +10:00 committed by Andrew Tridgell
parent 8ce97af40e
commit 12d639fc17
3 changed files with 12 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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();

13
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -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;

4
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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
};