Smoothen external flight mode GotoControl to Mission transitions (#26254)

src/modules/mc_pos_control/GotoControl/GotoControl.cpp

@@ -42,6 +42,7 @@
 #include <lib/mathlib/mathlib.h>
 
 using namespace time_literals;
+using namespace matrix;
 
 bool GotoControl::checkForSetpoint(const hrt_abstime &now, const bool enabled)
 {
@@ -64,10 +65,11 @@ bool GotoControl::checkForSetpoint(const hrt_abstime &now, const bool enabled)
 	return need_to_run;
 }
 
-void GotoControl::update(const float dt, const matrix::Vector3f &position, const float heading)
+void GotoControl::update(const float dt, const Vector3f &position, const Vector3f &velocity, const Vector3f &acceleration,
+			 const float heading)
 {
 	if (!_is_initialized) {
-		resetPositionSmoother(position);
+		resetPositionSmoother(position, velocity, acceleration);
 		resetHeadingSmoother(heading);
 		_is_initialized = true;
 	}
@@ -87,7 +89,7 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
 	}
 
 	if (_need_smoother_reset) {
-		resetPositionSmoother(position);
+		resetPositionSmoother(position, velocity, acceleration);
 	}
 
 	setPositionSmootherLimits(_goto_setpoint);
@@ -138,7 +140,7 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const
 	_vehicle_constraints_pub.publish(vehicle_constraints);
 }
 
-void GotoControl::resetPositionSmoother(const matrix::Vector3f &position)
+void GotoControl::resetPositionSmoother(const Vector3f &position, const Vector3f &velocity, const Vector3f &acceleration)
 {
 	if (!position.isAllFinite()) {
 		// TODO: error messaging
@@ -146,9 +148,7 @@ void GotoControl::resetPositionSmoother(const matrix::Vector3f &position)
 		return;
 	}
 
-	const Vector3f initial_acceleration{};
-	const Vector3f initial_velocity{};
-	_position_smoothing.reset(initial_acceleration, initial_velocity, position);
+	_position_smoothing.reset(acceleration, velocity, position);
 
 	_need_smoother_reset = false;
 }

src/modules/mc_pos_control/GotoControl/GotoControl.hpp

@@ -64,9 +64,11 @@ public:
 	/**
 	 * @brief resets the position smoother at the current position with zero velocity and acceleration.
 	 *
-	 * @param position [m] (NED) local vehicle position
+	 * @param position [m] vehicle position state NED local frame
+	 * @param velocity [m/s] vehicle velocity state
+	 * @param acceleration [m/s^2] vehicle acceleration state
 	 */
-	void resetPositionSmoother(const matrix::Vector3f &position);
+	void resetPositionSmoother(const matrix::Vector3f &position, const matrix::Vector3f &velocity, const matrix::Vector3f &acceleration);
 
 	/**
 	 * @brief resets the heading smoother at the current heading with zero heading rate and acceleration.
@@ -80,12 +82,15 @@ public:
 	 * loops to track.
 	 *
 	 * @param[in] dt [s] time since last control update
-	 * @param[in] position [m] (NED) local vehicle position
+	 * @param[in] position [m] vehicle position state NED local frame
+	 * @param[in] velocity [m/s] vehicle velocity state
+	 * @param[in] acceleration [m/s^2] vehicle acceleration state
 	 * @param[in] heading [rad] (from North) vehicle heading
 	 * @param[in] goto_setpoint struct containing current go-to setpoints
 	 * @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints
 	 */
-	void update(const float dt, const matrix::Vector3f &position, const float heading);
+	void update(const float dt, const matrix::Vector3f &position, const matrix::Vector3f &velocity, const matrix::Vector3f &acceleration,
+		    const float heading);
 
 	// Setting all parameters from the outside saves 300bytes flash
 	void setParamMpcAccHor(const float param_mpc_acc_hor) { _param_mpc_acc_hor = param_mpc_acc_hor; }

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -433,7 +433,7 @@ void MulticopterPositionControl::Run()
 						  && !_trajectory_setpoint_sub.updated();
 
 		if (_goto_control.checkForSetpoint(vehicle_local_position.timestamp_sample, goto_setpoint_enable)) {
-			_goto_control.update(dt, states.position, states.yaw);
+			_goto_control.update(dt, states.position, states.velocity, states.acceleration, states.yaw);
 		}
 
 		_trajectory_setpoint_sub.update(&_setpoint);