Skip to content

Commit

Permalink
AP_NavEKF3: remove unused assignments of posCheckPassed
Browse files Browse the repository at this point in the history
... and more-tightly scope variables to avoid similar problems in future

../../libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp:898:17: warning: Value stored to 'posCheckPassed' is never read [deadcode.DeadStores]
                posCheckPassed = true;
                ^                ~~~~
../../libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp:946:17: warning: Value stored to 'posCheckPassed' is never read [deadcode.DeadStores]
                posCheckPassed = true;
                ^                ~~~~
  • Loading branch information
peterbarker committed Feb 25, 2025
1 parent cfa1459 commit e2fdb81
Showing 1 changed file with 3 additions and 7 deletions.
10 changes: 3 additions & 7 deletions libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -682,11 +682,6 @@ void NavEKF3_core::SelectVelPosFusion()
// fuse selected position, velocity and height measurements
void NavEKF3_core::FuseVelPosNED()
{
// health is set bad until test passed
bool velCheckPassed = false; // boolean true if velocity measurements have passed innovation consistency checks
bool posCheckPassed = false; // boolean true if position measurements have passed innovation consistency check
bool hgtCheckPassed = false; // boolean true if height measurements have passed innovation consistency check

// declare variables used to control access to arrays
bool fuseData[6] {};
uint8_t stateIndex;
Expand Down Expand Up @@ -813,6 +808,7 @@ void NavEKF3_core::FuseVelPosNED()
ftype maxPosInnov2 = sq(MAX(0.01 * (ftype)frontend->_gpsPosInnovGate, 1.0))*(varInnovVelPos[3] + varInnovVelPos[4]);

posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
bool posCheckPassed = false; // boolean true if position measurements have passed innovation consistency check
if (posTestRatio < 1.0f || (PV_AidingMode == AID_NONE)) {
posCheckPassed = true;
lastGpsPosPassTime_ms = imuSampleTime_ms;
Expand Down Expand Up @@ -887,14 +883,14 @@ void NavEKF3_core::FuseVelPosNED()
varVelSum += varInnovVelPos[i];
}
velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01 * (ftype)frontend->_gpsVelInnovGate, 1.0)));
bool velCheckPassed = false; // boolean true if velocity measurements have passed innovation consistency checks
if (velTestRatio < 1.0) {
velCheckPassed = true;
lastVelPassTime_ms = imuSampleTime_ms;
} else if (frontend->_gpsGlitchRadiusMax <= 0) {
// 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
// to a test ratio of 1.
posCheckPassed = true;
lastGpsPosPassTime_ms = imuSampleTime_ms;
for (uint8_t i = 0; i<=imax; i++) {
varInnovVelPos[i] *= velTestRatio;
Expand Down Expand Up @@ -935,14 +931,14 @@ void NavEKF3_core::FuseVelPosNED()
// bias errors without rejecting the height sensor.
const bool onGroundNotNavigating = (PV_AidingMode == AID_NONE) && onGround;
const float maxTestRatio = onGroundNotNavigating ? 3.0f : 1.0f;
bool hgtCheckPassed = false; // boolean true if height measurements have passed innovation consistency check
if (hgtTestRatio < maxTestRatio) {
hgtCheckPassed = true;
lastHgtPassTime_ms = imuSampleTime_ms;
} else if ((frontend->_gpsGlitchRadiusMax <= 0) && !onGroundNotNavigating && (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS)) {
// 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
// to a test ratio of 1.
posCheckPassed = true;
lastGpsPosPassTime_ms = imuSampleTime_ms;
varInnovVelPos[5] *= hgtTestRatio;
hgtCheckPassed = true;
Expand Down

0 comments on commit e2fdb81

Please sign in to comment.