ekf2: move generic functions to control.cpp

these functions aren't specific to GPS fusion

src/modules/ekf2/EKF/control.cpp

@@ -1229,3 +1229,50 @@ void Ekf::controlAuxVelFusion()
 
 	}
 }
+
+
+bool Ekf::isVelStateAlignedWithObs() const
+{
+	/* Do sanity check to see if the innovation failures is likely caused by a yaw angle error
+	 * by measuring the angle between the velocity estimate and the last velocity observation
+	 * Only use those vectors if their norm if they are larger than 4 times their noise standard deviation
+	 */
+	const float vel_obs_xy_norm_sq = _last_vel_obs.xy().norm_squared();
+	const float vel_state_xy_norm_sq = _state.vel.xy().norm_squared();
+
+	const float vel_obs_threshold_sq = fmaxf(sq(4.f) * (_last_vel_obs_var(0) + _last_vel_obs_var(1)), sq(0.4f));
+	const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), sq(0.4f));
+
+	if (vel_obs_xy_norm_sq > vel_obs_threshold_sq && vel_state_xy_norm_sq > vel_state_threshold_sq) {
+		const float obs_dot_vel = Vector2f(_last_vel_obs).dot(_state.vel.xy());
+		const float cos_sq = sq(obs_dot_vel) / (vel_state_xy_norm_sq * vel_obs_xy_norm_sq);
+
+		if (cos_sq < sq(cosf(math::radians(25.f))) || obs_dot_vel < 0.f) {
+			// The angle between the observation and the velocity estimate is greater than 25 degrees
+			return false;
+		}
+	}
+
+	return true;
+}
+
+bool Ekf::hasHorizontalAidingTimedOut() const
+{
+	return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
+	       && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
+	       && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
+	       && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
+}
+
+void Ekf::processVelPosResetRequest()
+{
+	if (_velpos_reset_request) {
+		resetVelocity();
+		resetHorizontalPosition();
+		_velpos_reset_request = false;
+
+		// Reset the timeout counters
+		_time_last_hor_pos_fuse = _time_last_imu;
+		_time_last_hor_vel_fuse = _time_last_imu;
+	}
+}

src/modules/ekf2/EKF/gps_control.cpp

@@ -188,39 +188,6 @@ bool Ekf::shouldResetGpsFusion() const
 	return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure);
 }
 
-bool Ekf::hasHorizontalAidingTimedOut() const
-{
-	return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
-	       && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
-	       && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-	       && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
-}
-
-bool Ekf::isVelStateAlignedWithObs() const
-{
-	/* Do sanity check to see if the innovation failures is likely caused by a yaw angle error
-	 * by measuring the angle between the velocity estimate and the last velocity observation
-	 * Only use those vectors if their norm if they are larger than 4 times their noise standard deviation
-	 */
-	const float vel_obs_xy_norm_sq = _last_vel_obs.xy().norm_squared();
-	const float vel_state_xy_norm_sq = _state.vel.xy().norm_squared();
-
-	const float vel_obs_threshold_sq = fmaxf(sq(4.f) * (_last_vel_obs_var(0) + _last_vel_obs_var(1)), sq(0.4f));
-	const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), sq(0.4f));
-
-	if (vel_obs_xy_norm_sq > vel_obs_threshold_sq && vel_state_xy_norm_sq > vel_state_threshold_sq) {
-		const float obs_dot_vel = Vector2f(_last_vel_obs).dot(_state.vel.xy());
-		const float cos_sq = sq(obs_dot_vel) / (vel_state_xy_norm_sq * vel_obs_xy_norm_sq);
-
-		if (cos_sq < sq(cosf(math::radians(25.f))) || obs_dot_vel < 0.f) {
-			// The angle between the observation and the velocity estimate is greater than 25 degrees
-			return false;
-		}
-	}
-
-	return true;
-}
-
 void Ekf::processYawEstimatorResetRequest()
 {
 	/* The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight.
@@ -240,16 +207,3 @@ void Ekf::processYawEstimatorResetRequest()
 		}
 	}
 }
-
-void Ekf::processVelPosResetRequest()
-{
-	if (_velpos_reset_request) {
-		resetVelocity();
-		resetHorizontalPosition();
-		_velpos_reset_request = false;
-
-		// Reset the timeout counters
-		_time_last_hor_pos_fuse = _time_last_imu;
-		_time_last_hor_vel_fuse = _time_last_imu;
-	}
-}