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

AP_NavEKF3: replace AP_HAL::millis() with dal.millis()

This commit is contained in:
Randy Mackay 2022-05-17 20:22:36 +09:00
parent 9f9c5c30ae
commit 72f0de65d2
3 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -612,7 +612,7 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
// calculate innovation for Y observation
flowInnov[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y;
flowInnovTime_ms = AP_HAL::millis();
flowInnovTime_ms = dal.millis();
// calculate Kalman gains for the Y-axis observation
Kfusion[0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);

6
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -461,7 +461,7 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
switch (source) {
case AP_NavEKF_Source::SourceXY::GPS:
// check for timeouts
if (AP_HAL::millis() - gpsVelInnovTime_ms > 500) {
if (dal.millis() - gpsVelInnovTime_ms > 500) {
return false;
}
innovations = gpsVelInnov.tofloat();
@ -470,7 +470,7 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
#if EK3_FEATURE_EXTERNAL_NAV
case AP_NavEKF_Source::SourceXY::EXTNAV:
// check for timeouts
if (AP_HAL::millis() - extNavVelInnovTime_ms > 500) {
if (dal.millis() - extNavVelInnovTime_ms > 500) {
return false;
}
innovations = extNavVelInnov.tofloat();
@ -479,7 +479,7 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour
#endif // EK3_FEATURE_EXTERNAL_NAV
case AP_NavEKF_Source::SourceXY::OPTFLOW:
// check for timeouts
if (AP_HAL::millis() - flowInnovTime_ms > 500) {
if (dal.millis() - flowInnovTime_ms > 500) {
return false;
}
innovations.x = flowInnov[0];

4
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -442,7 +442,7 @@ void NavEKF3_core::SelectVelPosFusion()
CalculateVelInnovationsAndVariances(extNavVelDelayed.vel, extNavVelDelayed.err, frontend->extNavVelVarAccScale, extNavVelInnov, extNavVelVarInnov);
// record time innovations were calculated (for timeout checks)
extNavVelInnovTime_ms = AP_HAL::millis();
extNavVelInnovTime_ms = dal.millis();
}
#endif // EK3_FEATURE_EXTERNAL_NAV
@ -457,7 +457,7 @@ void NavEKF3_core::SelectVelPosFusion()
// calculate innovations and variances for reporting purposes only
CalculateVelInnovationsAndVariances(gpsDataDelayed.vel, frontend->_gpsHorizVelNoise, frontend->gpsNEVelVarAccScale, gpsVelInnov, gpsVelVarInnov);
// record time innovations were calculated (for timeout checks)
gpsVelInnovTime_ms = AP_HAL::millis();
gpsVelInnovTime_ms = dal.millis();
}
// detect position source changes. Trigger position reset if position source is valid