From 1eaa725854fbd5e5cb94cf5708a9ff2f71c1a40c Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 2 Jan 2026 11:56:39 -0600 Subject: [PATCH] Add DEBUG_POS_EST logging for GPS position residual analysis MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Enables comprehensive GPS update debugging for Issue #11202 investigation. When debug_mode = 76 (DEBUG_POS_EST), captures during GPS updates (~5Hz): - debug[0]: GPS EPH from receiver (cm × 100) - debug[1]: Unfiltered EPH input = MAX(GPS_eph, residual) (cm × 100) - debug[2]: Position residual magnitude (cm × 100) - debug[3]: GPS position weight × 10000 - debug[4]: Position residual X component (cm) - debug[5]: Position residual Y component (cm) - debug[6]: Current estimated EPH before update (cm × 100) - debug[7]: New EPH after filtering (cm × 100) This provides direct measurement of: 1. How far position estimate diverges from GPS (residual) 2. How much GPS is trusted during updates (weight) 3. Unfiltered vs filtered EPH values Critical for understanding position estimator behavior during high-G maneuvers where ~2m position divergence was observed. --- src/main/navigation/navigation_pos_estimator.c | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index cd861dacd6c..147bb5276f2 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -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) + 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) } return true;