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

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -61,8 +61,8 @@ bool FlightTaskAuto::activate(const trajectory_setpoint_s &last_setpoint)
 		// If the velocity setpoint is unknown, set to the current velocity
 		if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); }
 
-		// No acceleration estimate available, set to zero if the setpoint is NAN
+		// If accel setpoint unknown, set to the current accel
-		if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
+		if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = _acceleration(i); }
 	}
 
 	_position_smoothing.reset(accel_prev, vel_prev, pos_prev);

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -153,6 +153,17 @@ void FlightTask::_evaluateVehicleLocalPosition()
 			_velocity(2) = _sub_vehicle_local_position.get().vz;
 		}
 
+		// acceleration is calculated as the velocity derivative in EKF2,
+		// if velocity is available acceleration values are available
+		if (_sub_vehicle_local_position.get().v_xy_valid) {
+			_acceleration(0) = _sub_vehicle_local_position.get().ax;
+			_acceleration(1) = _sub_vehicle_local_position.get().ay;
+		}
+
+		if (_sub_vehicle_local_position.get().v_z_valid) {
+			_acceleration(2) = _sub_vehicle_local_position.get().az;
+		}
+
 		// distance to bottom
 		if (_sub_vehicle_local_position.get().dist_bottom_valid
 		    && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) {

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -201,6 +201,7 @@ protected:
 	/* Current vehicle state */
 	matrix::Vector3f _position; /**< current vehicle position */
 	matrix::Vector3f _velocity; /**< current vehicle velocity */
+	matrix::Vector3f _acceleration; /**< current vehicle acceleration (derivative of velocity) */
 
 	float _yaw{}; /**< current vehicle yaw heading */
 	float _unaided_yaw{};

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{};
+	_position_smoothing.reset(acceleration, velocity, position);
-	const Vector3f initial_velocity{};
-	_position_smoothing.reset(initial_acceleration, initial_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);