Add DEBUG_POS_EST logging for position residual debugging

[sensei-hacker]

User description

Summary

Debug build for investigating position estimate divergence during high-G maneuvers.

For debugging #11202

Changes

Adds comprehensive GPS update logging to DEBUG_POS_EST mode (76):

• Position residual magnitude and components (direct measurement, no coordinate conversion needed)
• GPS EPH from receiver
• Unfiltered EPH input before filtering
• GPS position weight (Kalman filter trust level)
• EPH before/after filtering

Purpose

Captures critical metrics to understand why position estimate diverges ~2m from GPS during 4.3G maneuvers:

• Real-time position residual (key diagnostic metric)
• GPS weight values (is GPS correction too weak?)
• Filtering behavior (unfiltered vs filtered EPH)

Testing

This is a debug/diagnostic build - not intended for merge to main branch.

Enables test flights to capture position estimator internal state for issue analysis.

Usage

set debug_mode = 76
save
# Fly high-G test pattern
# Analyze debug[0-7] fields in blackbox

Debug field mapping documented in commit message.

---

PR Type

Enhancement

---

Description

• Add comprehensive GPS update logging to DEBUG_POS_EST mode

• Capture position residual magnitude and components for diagnostics

• Log GPS weight and EPH filtering behavior for analysis

• Support investigation of position estimate divergence during high-G maneuvers

---

Diagram Walkthrough

flowchart LR
  GPS["GPS Update"] -->|Position Residual| DEBUG1["DEBUG_POS_EST Logging"]
  GPS -->|EPH Values| DEBUG1
  GPS -->|GPS Weight| DEBUG1
  DEBUG1 -->|8 Debug Fields| BLACKBOX["Blackbox Recording"]
  BLACKBOX -->|Analysis| ISSUE["Issue #11202 Investigation"]

Loading

File Walkthrough

Relevant files

Enhancement

navigation_pos_estimator.c Add GPS update debug logging for position estimator            src/main/navigation/navigation_pos_estimator.c Extract gps_eph_input calculation into variable for clarity and reuse Add 8 DEBUG_SET calls to log GPS update metrics during position estimation Log GPS EPH, unfiltered EPH input, position residuals, GPS weight, and filtered EPH Include detailed comments documenting debug field mapping and purpose +14/-1

---

[qodo-code-review]

PR Compliance Guide 🔍

All compliance sections have been disabled in the configurations.

[qodo-code-review]

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)

[qodo-code-review]

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));