ekf2: move controlGpsYawFusion() control.cpp -> gps_control.cpp

src/modules/ekf2/EKF/control.cpp

@@ -218,93 +218,6 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	updateDeadReckoningStatus();
 }
 
-void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
-{
-	if (!(_params.gnss_ctrl & GnssCtrl::YAW)
-	    || _control_status.flags.gps_yaw_fault) {
-
-		stopGpsYawFusion();
-		return;
-	}
-
-	updateGpsYaw(gps_sample);
-
-	const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
-
-	if (is_new_data_available) {
-
-		const bool continuing_conditions_passing = !gps_checks_failing;
-
-		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
-
-		const bool starting_conditions_passing = continuing_conditions_passing
-				&& _control_status.flags.tilt_align
-				&& gps_checks_passing
-				&& !is_gps_yaw_data_intermittent
-				&& !_gps_intermittent;
-
-		if (_control_status.flags.gps_yaw) {
-
-			if (continuing_conditions_passing) {
-
-				fuseGpsYaw();
-
-				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
-
-				if (is_fusion_failing) {
-					if (_nb_gps_yaw_reset_available > 0) {
-						// Data seems good, attempt a reset
-						resetYawToGps(gps_sample.yaw);
-
-						if (_control_status.flags.in_air) {
-							_nb_gps_yaw_reset_available--;
-						}
-
-					} else if (starting_conditions_passing) {
-						// Data seems good, but previous reset did not fix the issue
-						// something else must be wrong, declare the sensor faulty and stop the fusion
-						_control_status.flags.gps_yaw_fault = true;
-						stopGpsYawFusion();
-
-					} else {
-						// A reset did not fix the issue but all the starting checks are not passing
-						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-						stopGpsYawFusion();
-					}
-
-					// TODO: should we give a new reset credit when the fusion does not fail for some time?
-				}
-
-			} else {
-				// Stop GPS yaw fusion but do not declare it faulty
-				stopGpsYawFusion();
-			}
-
-		} else {
-			if (starting_conditions_passing) {
-				// Try to activate GPS yaw fusion
-				startGpsYawFusion(gps_sample);
-
-				if (_control_status.flags.gps_yaw) {
-					_nb_gps_yaw_reset_available = 1;
-				}
-			}
-		}
-
-	} else if (_control_status.flags.gps_yaw && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
-		// No yaw data in the message anymore. Stop until it comes back.
-		stopGpsYawFusion();
-	}
-
-	// Before takeoff, we do not want to continue to rely on the current heading
-	// if we had to stop the fusion
-	if (!_control_status.flags.in_air
-	    && !_control_status.flags.gps_yaw
-	    && _control_status_prev.flags.gps_yaw) {
-		_control_status.flags.yaw_align = false;
-	}
-}
-
 void Ekf::controlAirDataFusion()
 {
 	// control activation and initialisation/reset of wind states required for airspeed fusion

src/modules/ekf2/EKF/gps_control.cpp

@@ -262,3 +262,90 @@ bool Ekf::isYawFailure() const
 
 	return fabsf(yaw_error) > math::radians(25.f);
 }
+
+void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
+{
+	if (!(_params.gnss_ctrl & GnssCtrl::YAW)
+	    || _control_status.flags.gps_yaw_fault) {
+
+		stopGpsYawFusion();
+		return;
+	}
+
+	updateGpsYaw(gps_sample);
+
+	const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
+
+	if (is_new_data_available) {
+
+		const bool continuing_conditions_passing = !gps_checks_failing;
+
+		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
+
+		const bool starting_conditions_passing = continuing_conditions_passing
+				&& _control_status.flags.tilt_align
+				&& gps_checks_passing
+				&& !is_gps_yaw_data_intermittent
+				&& !_gps_intermittent;
+
+		if (_control_status.flags.gps_yaw) {
+
+			if (continuing_conditions_passing) {
+
+				fuseGpsYaw();
+
+				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
+
+				if (is_fusion_failing) {
+					if (_nb_gps_yaw_reset_available > 0) {
+						// Data seems good, attempt a reset
+						resetYawToGps(gps_sample.yaw);
+
+						if (_control_status.flags.in_air) {
+							_nb_gps_yaw_reset_available--;
+						}
+
+					} else if (starting_conditions_passing) {
+						// Data seems good, but previous reset did not fix the issue
+						// something else must be wrong, declare the sensor faulty and stop the fusion
+						_control_status.flags.gps_yaw_fault = true;
+						stopGpsYawFusion();
+
+					} else {
+						// A reset did not fix the issue but all the starting checks are not passing
+						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+						stopGpsYawFusion();
+					}
+
+					// TODO: should we give a new reset credit when the fusion does not fail for some time?
+				}
+
+			} else {
+				// Stop GPS yaw fusion but do not declare it faulty
+				stopGpsYawFusion();
+			}
+
+		} else {
+			if (starting_conditions_passing) {
+				// Try to activate GPS yaw fusion
+				startGpsYawFusion(gps_sample);
+
+				if (_control_status.flags.gps_yaw) {
+					_nb_gps_yaw_reset_available = 1;
+				}
+			}
+		}
+
+	} else if (_control_status.flags.gps_yaw && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
+		// No yaw data in the message anymore. Stop until it comes back.
+		stopGpsYawFusion();
+	}
+
+	// Before takeoff, we do not want to continue to rely on the current heading
+	// if we had to stop the fusion
+	if (!_control_status.flags.in_air
+	    && !_control_status.flags.gps_yaw
+	    && _control_status_prev.flags.gps_yaw) {
+		_control_status.flags.yaw_align = false;
+	}
+}