move implementation of updateAidSrcStatus to cpp file, saves flash

src/modules/ekf2/EKF/ekf.h

@@ -1135,80 +1135,7 @@ private:
 	void updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t &timestamp_sample,
 				   const float &observation, const float &observation_variance,
 				   const float &innovation, const float &innovation_variance,
-				   float innovation_gate = 1.f) const
+				   float innovation_gate = 1.f) const;
-	{
-		bool innovation_rejected = false;
-
-		const float test_ratio = sq(innovation) / (sq(innovation_gate) * innovation_variance);
-
-		if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) {
-
-			const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f);
-
-			static constexpr float tau = 0.5f;
-			const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f);
-
-			// test_ratio_filtered
-			if (PX4_ISFINITE(status.test_ratio_filtered)) {
-				status.test_ratio_filtered += alpha * (matrix::sign(innovation) * test_ratio - status.test_ratio_filtered);
-
-			} else {
-				// otherwise, init the filtered test ratio
-				status.test_ratio_filtered = test_ratio;
-			}
-
-			// innovation_filtered
-			if (PX4_ISFINITE(status.innovation_filtered)) {
-				status.innovation_filtered += alpha * (innovation - status.innovation_filtered);
-
-			} else {
-				// otherwise, init the filtered innovation
-				status.innovation_filtered = innovation;
-			}
-
-
-			// limit extremes in filtered values
-			static constexpr float kNormalizedInnovationLimit = 2.f;
-			static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit);
-
-			if (test_ratio > kTestRatioLimit) {
-
-				status.test_ratio_filtered = math::constrain(status.test_ratio_filtered, -kTestRatioLimit, kTestRatioLimit);
-
-				const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance);
-				status.innovation_filtered = math::constrain(status.innovation_filtered, -innov_limit, innov_limit);
-			}
-
-		} else {
-			// invalid timestamp_sample, reset
-			status.test_ratio_filtered = test_ratio;
-			status.innovation_filtered = innovation;
-		}
-
-		status.test_ratio = test_ratio;
-
-		status.observation = observation;
-		status.observation_variance = observation_variance;
-
-		status.innovation = innovation;
-		status.innovation_variance = innovation_variance;
-
-		if ((test_ratio > 1.f)
-		    || !PX4_ISFINITE(test_ratio)
-		    || !PX4_ISFINITE(status.innovation)
-		    || !PX4_ISFINITE(status.innovation_variance)
-		   ) {
-			innovation_rejected = true;
-		}
-
-		status.timestamp_sample = timestamp_sample;
-
-		// if any of the innovations are rejected, then the overall innovation is rejected
-		status.innovation_rejected = innovation_rejected;
-
-		// reset
-		status.fused = false;
-	}
 
 	// state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc)
 	template <typename T>

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1116,3 +1116,81 @@ bool Ekf::measurementUpdate(VectorState &K, const VectorState &H, const float R,
 	fuse(K, innovation);
 	return true;
 }
+
+void Ekf::updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t &timestamp_sample,
+				const float &observation, const float &observation_variance,
+				const float &innovation, const float &innovation_variance,
+				float innovation_gate) const
+{
+	bool innovation_rejected = false;
+
+	const float test_ratio = sq(innovation) / (sq(innovation_gate) * innovation_variance);
+
+	if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) {
+
+		const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f);
+
+		static constexpr float tau = 0.5f;
+		const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f);
+
+		// test_ratio_filtered
+		if (PX4_ISFINITE(status.test_ratio_filtered)) {
+			status.test_ratio_filtered += alpha * (matrix::sign(innovation) * test_ratio - status.test_ratio_filtered);
+
+		} else {
+			// otherwise, init the filtered test ratio
+			status.test_ratio_filtered = test_ratio;
+		}
+
+		// innovation_filtered
+		if (PX4_ISFINITE(status.innovation_filtered)) {
+			status.innovation_filtered += alpha * (innovation - status.innovation_filtered);
+
+		} else {
+			// otherwise, init the filtered innovation
+			status.innovation_filtered = innovation;
+		}
+
+
+		// limit extremes in filtered values
+		static constexpr float kNormalizedInnovationLimit = 2.f;
+		static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit);
+
+		if (test_ratio > kTestRatioLimit) {
+
+			status.test_ratio_filtered = math::constrain(status.test_ratio_filtered, -kTestRatioLimit, kTestRatioLimit);
+
+			const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance);
+			status.innovation_filtered = math::constrain(status.innovation_filtered, -innov_limit, innov_limit);
+		}
+
+	} else {
+		// invalid timestamp_sample, reset
+		status.test_ratio_filtered = test_ratio;
+		status.innovation_filtered = innovation;
+	}
+
+	status.test_ratio = test_ratio;
+
+	status.observation = observation;
+	status.observation_variance = observation_variance;
+
+	status.innovation = innovation;
+	status.innovation_variance = innovation_variance;
+
+	if ((test_ratio > 1.f)
+	    || !PX4_ISFINITE(test_ratio)
+	    || !PX4_ISFINITE(status.innovation)
+	    || !PX4_ISFINITE(status.innovation_variance)
+	   ) {
+		innovation_rejected = true;
+	}
+
+	status.timestamp_sample = timestamp_sample;
+
+	// if any of the innovations are rejected, then the overall innovation is rejected
+	status.innovation_rejected = innovation_rejected;
+
+	// reset
+	status.fused = false;
+}