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

AP_NavEKF3: rename lastPosPassTime_ms to lastGpsPosPassTime_ms

Browse Source
This commit is contained in:
Peter Barker 2024-08-26 14:22:40 +10:00 committed by Andrew Tridgell
parent a51614f360
commit 4782ace3b9
5 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -241,7 +241,7 @@ void NavEKF3_core::SelectBetaDragFusion()
bool f_timeTrigger = ((imuSampleTime_ms - prevBetaDragStep_ms) >= frontend->betaAvg_ms); bool f_timeTrigger = ((imuSampleTime_ms - prevBetaDragStep_ms) >= frontend->betaAvg_ms);
// use of air data to constrain drift is necessary if we have limited sensor data or are doing inertial dead reckoning // use of air data to constrain drift is necessary if we have limited sensor data or are doing inertial dead reckoning
bool is_dead_reckoning = ((imuSampleTime_ms - lastPosPassTime_ms) > frontend->deadReckonDeclare_ms) && ((imuSampleTime_ms - lastVelPassTime_ms) > frontend->deadReckonDeclare_ms); bool is_dead_reckoning = ((imuSampleTime_ms - lastGpsPosPassTime_ms) > frontend->deadReckonDeclare_ms) && ((imuSampleTime_ms - lastVelPassTime_ms) > frontend->deadReckonDeclare_ms);
const bool noYawSensor = !use_compass() && !using_noncompass_for_yaw(); const bool noYawSensor = !use_compass() && !using_noncompass_for_yaw();
const bool f_required = (noYawSensor && (frontend->_betaMask & (1<<1))) || is_dead_reckoning; const bool f_required = (noYawSensor && (frontend->_betaMask & (1<<1))) || is_dead_reckoning;

10
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -336,7 +336,7 @@ void NavEKF3_core::setAidingMode()
#endif #endif
// Check if GPS or external nav is being used // Check if GPS or external nav is being used
bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms); bool posUsed = (imuSampleTime_ms - lastGpsPosPassTime_ms <= minTestTime_ms);
bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms); bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
// Check if attitude drift has been constrained by a measurement source // Check if attitude drift has been constrained by a measurement source
@ -365,7 +365,7 @@ void NavEKF3_core::setAidingMode()
#if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BEACON_FUSION
(imuSampleTime_ms - rngBcn.lastPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - rngBcn.lastPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
#endif #endif
(imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastGpsPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms); (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
} }
@ -382,7 +382,7 @@ void NavEKF3_core::setAidingMode()
#if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BEACON_FUSION
(imuSampleTime_ms - rngBcn.lastPassTime_ms > maxLossTime_ms) && (imuSampleTime_ms - rngBcn.lastPassTime_ms > maxLossTime_ms) &&
#endif #endif
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); (imuSampleTime_ms - lastGpsPosPassTime_ms > maxLossTime_ms);
} }
if (attAidLossCritical) { if (attAidLossCritical) {
@ -488,7 +488,7 @@ void NavEKF3_core::setAidingMode()
velTimeout = false; velTimeout = false;
// reset the last fusion accepted times to prevent unwanted activation of timeout logic // reset the last fusion accepted times to prevent unwanted activation of timeout logic
lastPosPassTime_ms = imuSampleTime_ms; lastGpsPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms;
#if EK3_FEATURE_BEACON_FUSION #if EK3_FEATURE_BEACON_FUSION
rngBcn.lastPassTime_ms = imuSampleTime_ms; rngBcn.lastPassTime_ms = imuSampleTime_ms;
@ -786,7 +786,7 @@ void NavEKF3_core::updateFilterStatus(void)
status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
status.flags.takeoff = dal.get_takeoff_expected(); // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator status.flags.takeoff = dal.get_takeoff_expected(); // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator
status.flags.touchdown = dal.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode status.flags.touchdown = dal.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
status.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); status.flags.using_gps = ((imuSampleTime_ms - lastGpsPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
status.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy status.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy
status.flags.gps_quality_good = gpsGoodToAlign; status.flags.gps_quality_good = gpsGoodToAlign;
// for reporting purposes we report rejecting airspeed after 3s of not fusing when we want to fuse the data // for reporting purposes we report rejecting airspeed after 3s of not fusing when we want to fuse the data

14
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -141,7 +141,7 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
// clear the timeout flags and counters // clear the timeout flags and counters
posTimeout = false; posTimeout = false;
lastPosPassTime_ms = imuSampleTime_ms; lastGpsPosPassTime_ms = imuSampleTime_ms;
} }
#if EK3_FEATURE_POSITION_RESET #if EK3_FEATURE_POSITION_RESET
@ -150,7 +150,7 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
// Returns true if the set was successful // Returns true if the set was successful
bool NavEKF3_core::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms) bool NavEKF3_core::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
{ {
if ((imuSampleTime_ms - lastPosPassTime_ms) < frontend->deadReckonDeclare_ms || if ((imuSampleTime_ms - lastGpsPosPassTime_ms) < frontend->deadReckonDeclare_ms ||
(PV_AidingMode == AID_NONE) (PV_AidingMode == AID_NONE)
|| !validOrigin) { || !validOrigin) {
return false; return false;
@ -775,17 +775,17 @@ void NavEKF3_core::FuseVelPosNED()
posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2; posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
if (posTestRatio < 1.0f || (PV_AidingMode == AID_NONE)) { if (posTestRatio < 1.0f || (PV_AidingMode == AID_NONE)) {
posCheckPassed = true; posCheckPassed = true;
lastPosPassTime_ms = imuSampleTime_ms; lastGpsPosPassTime_ms = imuSampleTime_ms;
} else if ((frontend->_gpsGlitchRadiusMax <= 0) && (PV_AidingMode != AID_NONE)) { } else if ((frontend->_gpsGlitchRadiusMax <= 0) && (PV_AidingMode != AID_NONE)) {
// Handle the special case where the glitch radius parameter has been set to a non-positive number. // Handle the special case where the glitch radius parameter has been set to a non-positive number.
// The innovation variance is increased to limit the state update to an amount corresponding // The innovation variance is increased to limit the state update to an amount corresponding
// to a test ratio of 1. // to a test ratio of 1.
posCheckPassed = true; posCheckPassed = true;
lastPosPassTime_ms = imuSampleTime_ms; lastGpsPosPassTime_ms = imuSampleTime_ms;
varInnovVelPos[3] *= posTestRatio; varInnovVelPos[3] *= posTestRatio;
varInnovVelPos[4] *= posTestRatio; varInnovVelPos[4] *= posTestRatio;
posCheckPassed = true; posCheckPassed = true;
lastPosPassTime_ms = imuSampleTime_ms; lastGpsPosPassTime_ms = imuSampleTime_ms;
} }
// Use position data if healthy or timed out or bad IMU data // Use position data if healthy or timed out or bad IMU data
@ -856,7 +856,7 @@ void NavEKF3_core::FuseVelPosNED()
// The innovation variance is increased to limit the state update to an amount corresponding // The innovation variance is increased to limit the state update to an amount corresponding
// to a test ratio of 1. // to a test ratio of 1.
posCheckPassed = true; posCheckPassed = true;
lastPosPassTime_ms = imuSampleTime_ms; lastGpsPosPassTime_ms = imuSampleTime_ms;
for (uint8_t i = 0; i<=imax; i++) { for (uint8_t i = 0; i<=imax; i++) {
varInnovVelPos[i] *= velTestRatio; varInnovVelPos[i] *= velTestRatio;
} }
@ -904,7 +904,7 @@ void NavEKF3_core::FuseVelPosNED()
// The innovation variance is increased to limit the state update to an amount corresponding // The innovation variance is increased to limit the state update to an amount corresponding
// to a test ratio of 1. // to a test ratio of 1.
posCheckPassed = true; posCheckPassed = true;
lastPosPassTime_ms = imuSampleTime_ms; lastGpsPosPassTime_ms = imuSampleTime_ms;
varInnovVelPos[5] *= hgtTestRatio; varInnovVelPos[5] *= hgtTestRatio;
hgtCheckPassed = true; hgtCheckPassed = true;
lastHgtPassTime_ms = imuSampleTime_ms; lastHgtPassTime_ms = imuSampleTime_ms;

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -203,7 +203,7 @@ void NavEKF3_core::InitialiseVariables()
prevBetaDragStep_ms = imuSampleTime_ms; prevBetaDragStep_ms = imuSampleTime_ms;
lastBaroReceived_ms = imuSampleTime_ms; lastBaroReceived_ms = imuSampleTime_ms;
lastVelPassTime_ms = 0; lastVelPassTime_ms = 0;
lastPosPassTime_ms = 0; lastGpsPosPassTime_ms = 0;
lastHgtPassTime_ms = 0; lastHgtPassTime_ms = 0;
lastTasPassTime_ms = 0; lastTasPassTime_ms = 0;
lastSynthYawTime_ms = 0; lastSynthYawTime_ms = 0;

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1114,7 +1114,7 @@ private:
uint32_t lastBaroReceived_ms; // time last time we received baro height data uint32_t lastBaroReceived_ms; // time last time we received baro height data
uint16_t hgtRetryTime_ms; // time allowed without use of height measurements before a height timeout is declared uint16_t hgtRetryTime_ms; // time allowed without use of height measurements before a height timeout is declared
uint32_t lastVelPassTime_ms; // time stamp when GPS velocity measurement last passed innovation consistency check (msec) uint32_t lastVelPassTime_ms; // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec) uint32_t lastGpsPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec)
uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec) uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec)
uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec) uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec)
uint32_t lastTasFailTime_ms; // time stamp when airspeed measurement last failed innovation consistency check (msec) uint32_t lastTasFailTime_ms; // time stamp when airspeed measurement last failed innovation consistency check (msec)