Skip to content
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 14 additions & 1 deletion src/main/navigation/navigation_pos_estimator.c
Original file line number Diff line number Diff line change
Expand Up @@ -692,7 +692,20 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
ctx->accBiasCorr.y += (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v));

/* Adjust EPH */
ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p);
const float gps_eph_input = MAX(posEstimator.gps.eph, gpsPosResidualMag);
ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, gps_eph_input, w_xy_gps_p);

// DEBUG: GPS update metrics for Issue #11202 investigation
// These override the position/velocity debug values during GPS updates (~5Hz)
// Provides critical data: position residual, GPS weight, unfiltered EPH input
DEBUG_SET(DEBUG_POS_EST, 0, (int32_t)(posEstimator.gps.eph * 100)); // GPS EPH from receiver (cm)
DEBUG_SET(DEBUG_POS_EST, 1, (int32_t)(gps_eph_input * 100)); // Unfiltered EPH input = MAX(GPS_eph, residual) (cm)
DEBUG_SET(DEBUG_POS_EST, 2, (int32_t)(gpsPosResidualMag * 100)); // Position residual magnitude (cm)
DEBUG_SET(DEBUG_POS_EST, 3, (int32_t)(w_xy_gps_p * 10000)); // GPS position weight × 10000
DEBUG_SET(DEBUG_POS_EST, 4, (int32_t)(gpsPosXResidual)); // Position residual X (cm)
DEBUG_SET(DEBUG_POS_EST, 5, (int32_t)(gpsPosYResidual)); // Position residual Y (cm)
Comment on lines +705 to +706
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggestion: To ensure consistency with the comment indicating centimeters, multiply gpsPosXResidual and gpsPosYResidual by 100 before casting them for the debug output. [possible issue, importance: 7]

Suggested change
DEBUG_SET(DEBUG_POS_EST, 4, (int32_t)(gpsPosXResidual)); // Position residual X (cm)
DEBUG_SET(DEBUG_POS_EST, 5, (int32_t)(gpsPosYResidual)); // Position residual Y (cm)
DEBUG_SET(DEBUG_POS_EST, 4, (int32_t)(gpsPosXResidual * 100)); // Position residual X (cm)
DEBUG_SET(DEBUG_POS_EST, 5, (int32_t)(gpsPosYResidual * 100)); // Position residual Y (cm)

DEBUG_SET(DEBUG_POS_EST, 6, (int32_t)(posEstimator.est.eph * 100)); // Current estimated EPH before update (cm)
DEBUG_SET(DEBUG_POS_EST, 7, (int32_t)(ctx->newEPH * 100)); // New EPH after filtering (cm)
Comment on lines +701 to +708
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggestion: Protect the debug casts by checking for non-finite values and clamping to int32_t range to avoid undefined/implementation-defined behavior when NaN/Inf or huge values occur. [Learned best practice, importance: 5]

Suggested change
DEBUG_SET(DEBUG_POS_EST, 0, (int32_t)(posEstimator.gps.eph * 100)); // GPS EPH from receiver (cm)
DEBUG_SET(DEBUG_POS_EST, 1, (int32_t)(gps_eph_input * 100)); // Unfiltered EPH input = MAX(GPS_eph, residual) (cm)
DEBUG_SET(DEBUG_POS_EST, 2, (int32_t)(gpsPosResidualMag * 100)); // Position residual magnitude (cm)
DEBUG_SET(DEBUG_POS_EST, 3, (int32_t)(w_xy_gps_p * 10000)); // GPS position weight × 10000
DEBUG_SET(DEBUG_POS_EST, 4, (int32_t)(gpsPosXResidual)); // Position residual X (cm)
DEBUG_SET(DEBUG_POS_EST, 5, (int32_t)(gpsPosYResidual)); // Position residual Y (cm)
DEBUG_SET(DEBUG_POS_EST, 6, (int32_t)(posEstimator.est.eph * 100)); // Current estimated EPH before update (cm)
DEBUG_SET(DEBUG_POS_EST, 7, (int32_t)(ctx->newEPH * 100)); // New EPH after filtering (cm)
static inline int32_t debugFloatToI32(const float v)
{
if (!isfinite(v)) {
return 0;
}
if (v > (float)INT32_MAX) {
return INT32_MAX;
}
if (v < (float)INT32_MIN) {
return INT32_MIN;
}
return (int32_t)v;
}
DEBUG_SET(DEBUG_POS_EST, 0, debugFloatToI32(posEstimator.gps.eph * 100.0f));
DEBUG_SET(DEBUG_POS_EST, 1, debugFloatToI32(gps_eph_input * 100.0f));
DEBUG_SET(DEBUG_POS_EST, 2, debugFloatToI32(gpsPosResidualMag * 100.0f));
DEBUG_SET(DEBUG_POS_EST, 3, debugFloatToI32(w_xy_gps_p * 10000.0f));
DEBUG_SET(DEBUG_POS_EST, 6, debugFloatToI32(posEstimator.est.eph * 100.0f));
DEBUG_SET(DEBUG_POS_EST, 7, debugFloatToI32(ctx->newEPH * 100.0f));

}

return true;
Expand Down