fix(ekf2): break unbounded recursion

fuseDirectStateMeasurement() ends with a call to constrainStateVariances(),
which iterates every state and, for any whose variance has grown past its
limit, calls fuseDirectStateMeasurement() again to apply a corrective
fuse. That inner call then runs its own constrainStateVariances(),
repeating the loop.

Call chain:

  fuseDirectStateMeasurement                      ekf_helper.cpp:1030
   `- constrainStateVariances                     ekf_helper.cpp:1085
       `- constrainStateVar                       covariance.cpp:290
           `- fuseDirectStateMeasurement          covariance.cpp:302
               `- constrainStateVariances              (recursion)
                   `- ...

Under normal operation the P(i,i) > max branch is rarely taken and the
recursion stays shallow, however, while doing unrelated overnight testing
we saw a hard-fault with TCB corruption, presumably due to a stack overflow.

The trace pointed to fuseDirectStateMeasurement where Claude detected
the recursive call.

This fix breaks the recursion by gating the constrainStateVariances()
call on a new parameter, defaulting to true for existing callers.
constrainStateVar() passes false when it re-enters the fuse from the
constraint path, capping the worst-case recursion depth at 2.

src/modules/ekf2/EKF/covariance.cpp

@@ -296,10 +296,12 @@ void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
 		} else if (P(i, i) > max) {
 			// Constrain the variance growth by fusing zero innovation as clipping the variance
 			// would artifically increase the correlation between states and destabilize the filter.
+			// constrain_variances=false prevents unbounded recursion via
+			// fuseDirectStateMeasurement -> constrainStateVariances -> constrainStateVar -> here.
 			const float innov = 0.f;
 			const float R = 10.f * P(i, i); // This reduces the variance by ~10% as K = P / (P + R)
 			const float innov_var = P(i, i) + R;
-			fuseDirectStateMeasurement(innov, innov_var, R, i);
+			fuseDirectStateMeasurement(innov, innov_var, R, i, /*constrain_variances=*/false);
 		}
 	}
 }

src/modules/ekf2/EKF/ekf.h

@@ -263,7 +263,10 @@ public:
 	}
 
 	// fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc)
-	void fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index);
+	// constrain_variances must be false when called from inside constrainStateVar to prevent
+	// unbounded recursion (constrainStateVariances can call back into this function).
+	void fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index,
+					bool constrain_variances = true);
 
 	bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation);
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1027,7 +1027,8 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 	}
 }
 
-void Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index)
+void Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index,
+				     bool constrain_variances)
 {
 	VectorState K;  // Kalman gain vector for any single observation - sequential fusion is used.
 
@@ -1081,7 +1082,9 @@ void Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c
 
 #endif
 
-	constrainStateVariances();
+	if (constrain_variances) {
+		constrainStateVariances();
+	}
 
 	// apply the state corrections
 	fuse(K, innov);