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 (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
+		// If accel setpoint unknown, set to the current accel
+		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{};