added full support for standard vtol in SIH

Signed-off-by: RomanBapst <bapstroman@gmail.com>
added full support for external autostart scripts
2026-05-28 00:30:04 +08:00 · 2024-11-12 11:16:44 +03:00 · 2024-11-12 11:16:22 +03:00 · 2024-11-12 11:16:22 +03:00 · 2024-09-18 11:17:37 +02:00 · 2024-09-17 10:55:00 -07:00
60 changed files with 957 additions and 209 deletions
@@ -88,7 +88,7 @@ jobs:
      run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
    - name: Upload px4 coredump
      if: failure()
-      uses: actions/upload-artifact@v2-preview
+      uses: actions/upload-artifact@v4
      with:
        name: coredump
        path: px4.core
@@ -103,21 +103,21 @@ jobs:

    - name: Upload px4 binary
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: binary
        path: build/px4_sitl_default/bin/px4

    - name: Store PX4 log
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: px4_log
        path: ~/.ros/log/*/*.ulg

    - name: Store ROS log
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: ros_log
        path: ~/.ros/**/rostest-*.log
@@ -83,7 +83,7 @@ jobs:
      run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
    - name: Upload px4 coredump
      if: failure()
-      uses: actions/upload-artifact@v2-preview
+      uses: actions/upload-artifact@v4
      with:
        name: coredump
        path: px4.core
@@ -98,21 +98,21 @@ jobs:

    - name: Upload px4 binary
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: binary
        path: build/px4_sitl_default/bin/px4

    - name: Store PX4 log
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: px4_log
        path: ~/.ros/log/*/*.ulg

    - name: Store ROS log
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: ros_log
        path: ~/.ros/**/rostest-*.log
@@ -16,11 +16,13 @@ param set-default RD_WHEEL_TRACK 0.3
 param set-default RD_MAN_YAW_SCALE 0.1
 param set-default RD_MAX_ACCEL 6
 param set-default RD_MAX_JERK 30
-param set-default RD_MAX_SPEED 7
-param set-default RD_YAW_RATE_P 5
+param set-default RD_MAX_THR_YAW_R 5
+param set-default RD_YAW_RATE_P 0.1
 param set-default RD_YAW_RATE_I 0
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0
+param set-default RD_MAX_SPEED 5
+param set-default RD_MAX_THR_SPD 7
 param set-default RD_SPEED_P 1
 param set-default RD_SPEED_I 0
 param set-default RD_MAX_YAW_RATE 180
@@ -43,6 +43,10 @@ param set-default CA_SV_CS1_TRQ_P 0.3
 param set-default CA_SV_CS1_TRQ_Y -0.3
 param set-default CA_SV_CS1_TYPE 6

+param set-default FW_AIRSPD_MAX 12
+param set-default FW_AIRSPD_MIN 7
+param set-default FW_AIRSPD_TRIM 10
+
 param set-default HIL_ACT_FUNC1 101
 param set-default HIL_ACT_FUNC2 102
 param set-default HIL_ACT_FUNC5 202
@@ -62,6 +66,8 @@ param set-default CBRK_SUPPLY_CHK 894281
 # - without safety switch
 param set-default CBRK_IO_SAFETY 22027

+param set-default SENS_DPRES_OFF 0.001
+
 param set SIH_T_MAX 2.0
 param set SIH_Q_MAX 0.0165
 param set SIH_MASS 0.2
@@ -75,3 +81,5 @@ param set SIH_L_ROLL 0.145

 # sih as tailsitter
 param set SIH_VEHICLE_TYPE 2
+
+param set-default VT_ARSP_TRANS 6
@@ -0,0 +1,99 @@
+#!/bin/sh
+#
+# @name SIH Tailsitter Duo
+#
+# @type Simulation
+# @class VTOL
+#
+# @output Motor1 motor right
+# @output Motor2 motor left
+# @output Servo1 elevon right
+# @output Servo2 elevon left
+#
+# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
+#
+# @board px4_fmu-v2 exclude
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set UAVCAN_ENABLE 0
+param set-default VT_B_TRANS_DUR 5
+param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_TYPE 2
+param set-default MPC_MAN_Y_MAX 60
+param set-default MC_PITCH_P 5
+
+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR0_PX 0.2
+param set-default CA_ROTOR0_PY 0.2
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR1_PX -0.2
+param set-default CA_ROTOR1_PY -0.2
+param set-default CA_ROTOR2_PX 0.2
+param set-default CA_ROTOR2_PY -0.2
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.2
+param set-default CA_ROTOR3_PY 0.2
+param set-default CA_ROTOR3_KM -0.05
+
+
+param set-default CA_ROTOR4_PX -0.3
+param set-default CA_ROTOR4_KM 0.05
+param set-default CA_ROTOR4_AX 1
+param set-default CA_ROTOR4_AZ 0
+
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS0_TRQ_R 0.5
+param set-default CA_SV_CS0_TYPE 2
+param set-default CA_SV_CS1_TRQ_P 1
+param set-default CA_SV_CS1_TYPE 3
+param set-default CA_SV_CS2_TRQ_Y 1
+
+param set HIL_ACT_REV 32
+
+param set-default FW_AIRSPD_MAX 12
+param set-default FW_AIRSPD_MIN 7
+param set-default FW_AIRSPD_TRIM 10
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+param set-default HIL_ACT_FUNC5 201
+param set-default HIL_ACT_FUNC6 202
+param set-default HIL_ACT_FUNC7 203
+param set-default HIL_ACT_FUNC8 105
+
+param set-default MAV_TYPE 22
+
+
+
+# set SYS_HITL to 2 to start the SIH and avoid sensors startup
+param set-default SYS_HITL 2
+
+# disable some checks to allow to fly:
+# - without real battery
+param set-default CBRK_SUPPLY_CHK 894281
+# - without safety switch
+param set-default CBRK_IO_SAFETY 22027
+
+param set-default SENS_DPRES_OFF 0.001
+
+param set SIH_T_MAX 2.0
+param set SIH_Q_MAX 0.0165
+param set SIH_MASS 0.2
+# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
+param set SIH_IXX 0.00354
+param set SIH_IYY 0.000625
+param set SIH_IZZ 0.00300
+param set SIH_IXZ 0
+param set SIH_KDV 0.2
+param set SIH_L_ROLL 0.145
+
+# sih as tailsitter
+param set SIH_VEHICLE_TYPE 3
+
+param set-default VT_ARSP_TRANS 6
@@ -25,12 +25,14 @@ param set-default RBCLW_REV 1 # reverse right wheels
 param set-default RD_WHEEL_TRACK 0.3
 param set-default RD_MAN_YAW_SCALE 1
 param set-default RD_MAX_ACCEL 5
-param set-default RD_MAX_JERK 50
-param set-default RD_MAX_SPEED 2
+param set-default RD_MAX_JERK 10
+param set-default RD_MAX_THR_YAW_R 4
 param set-default RD_YAW_RATE_P 0.1
 param set-default RD_YAW_RATE_I 0
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0
+param set-default RD_MAX_SPEED 1.8
+param set-default RD_MAX_THR_SPD 2
 param set-default RD_SPEED_P 0.5
 param set-default RD_SPEED_I 0.1
 param set-default RD_MAX_YAW_RATE 300
@@ -48,6 +48,7 @@ if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
 		1100_rc_quad_x_sih.hil
 		1101_rc_plane_sih.hil
 		1102_tailsitter_duo_sih.hil
+		1103_standard_vtol_sih.hil
 	)
 endif()

@@ -105,6 +105,13 @@ then
 		fi
 	fi

+  if [ -e /fs/microsd/new ]
+  then
+    echo "Updating external autostart files"
+    rm -r $SDCARD_EXT_PATH
+    mv /fs/microsd/new $SDCARD_EXT_PATH
+  fi
+
 	# Check for an update of the ext_autostart folder, and replace the old one with it
 	if [ -e /fs/microsd/ext_autostart_new ]
 	then
@@ -39,7 +39,6 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -70,7 +69,6 @@ CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
-CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@@ -23,26 +23,27 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
-CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=n
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=n
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_OSD_MSP_OSD=y
-CONFIG_DRIVERS_POWER_MONITOR_INA226=y
+CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
-CONFIG_DRIVERS_UAVCAN=y
+CONFIG_DRIVERS_UAVCAN=n
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
@@ -73,6 +73,7 @@ set(msg_files
 	DebugVect.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
+	DistanceSensorModeChangeRequest.msg
 	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg
@@ -0,0 +1,5 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 request_on_off 			# request to disable/enable the distance sensor
+uint8 REQUEST_OFF = 0
+uint8 REQUEST_ON  = 1
@@ -30,7 +30,8 @@ bool loiter_direction_counter_clockwise # loiter direction is clockwise by defau
 float32 loiter_orientation 	# Orientation of the major axis with respect to true north in rad [-pi,pi)
 uint8 	loiter_pattern		# loitern pattern to follow

-float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
+float32 acceptance_radius   # horizontal acceptance_radius (meters)
+float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed wing guidance, NAN = let guidance choose (meters)

 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
@@ -1,11 +1,13 @@
 uint64 timestamp # time since system start (microseconds)

-float32 actual_speed	       # [m/s] Actual forward speed of the rover
-float32 actual_yaw_deg         # [deg] Actual yaw of the rover
-float32 actual_yaw_rate_deg_s  # [deg/s] Actual yaw rate of the rover
-float32 desired_yaw_rate_deg_s # [deg/s] Yaw rate setpoint for the closed loop yaw rate controller
-float32 pid_yaw_integral       # Integral of the PID for the closed loop yaw controller
-float32 pid_yaw_rate_integral  # Integral of the PID for the closed loop yaw rate controller
-float32 pid_throttle_integral  # Integral of the PID for the closed loop speed controller
+float32 actual_speed	         # [m/s] Actual forward speed of the rover
+float32 actual_yaw               # [rad] Actual yaw of the rover
+float32 actual_yaw_rate          # [rad/s] Actual yaw rate of the rover
+float32 desired_yaw_rate         # [rad/s] Yaw rate setpoint for the closed loop yaw rate controller
+float32 forward_speed_normalized # [-1, 1] Normalized forward speed setpoint
+float32 speed_diff_normalized    # [-1, 1] Normalized speed difference setpoint between the left and right motor
+float32 pid_yaw_integral         # Integral of the PID for the closed loop yaw controller
+float32 pid_yaw_rate_integral    # Integral of the PID for the closed loop yaw rate controller
+float32 pid_throttle_integral    # Integral of the PID for the closed loop speed controller

 # TOPICS rover_differential_status
@@ -27,3 +27,4 @@ float32 pitch_sp_rad			# Current pitch setpoint [rad]
 float32 throttle_trim			# estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions

 float32 underspeed_ratio		# 0: no underspeed, 1: maximal underspeed. Controller takes measures to avoid stall proportional to ratio if >0.
+float32 fast_descend_ratio 		#  value indicating if fast descend mode is enabled with ramp up and ramp down [0-1]
@@ -55,6 +55,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/distance_sensor_mode_change_request.h>

 using namespace time_literals;

@@ -143,6 +144,8 @@ private:
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN};
+	uORB::Subscription _dist_sense_mode_change_sub{ORB_ID(distance_sensor_mode_change_request)};
+	typeof(px4::msg::DistanceSensorModeChangeRequest::request_on_off) _req_mode{px4::msg::DistanceSensorModeChangeRequest::REQUEST_OFF};
 	bool _restriction{false};
 	bool _auto_restriction{false};
 	bool _prev_restriction{false};
@@ -412,6 +415,17 @@ void LightwareLaser::start()

 int LightwareLaser::updateRestriction()
 {
+	if (_dist_sense_mode_change_sub.updated()) {
+		distance_sensor_mode_change_request_s dist_sense_mode_change;
+
+		if (_dist_sense_mode_change_sub.copy(&dist_sense_mode_change)) {
+			_req_mode = dist_sense_mode_change.request_on_off;
+
+		} else {
+			_req_mode = distance_sensor_mode_change_request_s::REQUEST_OFF;
+		}
+	}
+
 	px4::msg::VehicleStatus vehicle_status;

 	if (_vehicle_status_sub.update(&vehicle_status)) {
@@ -452,7 +466,7 @@ int LightwareLaser::updateRestriction()
 		break;

 	case 2:
-		_restriction = _auto_restriction;
+		_restriction = _auto_restriction && _req_mode != distance_sensor_mode_change_request_s::REQUEST_ON;
 		break;
 	}

@@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
 *
 * @value 0 Disabled
 * @value 1 Enabled
- * @value 2 Disabled during VTOL fast forward flight
+ * @value 2 Enabled in VTOL MC mode, listen to request from system in FW mode
 *
 * @min 0
 * @max 2
@@ -116,6 +116,39 @@ public:
 		return self;
 	}

+	template<size_t MM, size_t NN>
+	Matrix<Type, P, Q> operator-(const SliceT<const Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
+	{
+		return Matrix<Type, P, Q> {*this} - other;
+	}
+
+
+	Matrix<Type, P, Q> operator-(const Matrix<Type, P, Q> &other)
+	{
+		return Matrix<Type, P, Q> {*this} - other;
+	}
+
+	Matrix<Type, P, Q> operator-(const Type &other)
+	{
+		return Matrix<Type, P, Q> {*this} - other;
+	}
+
+	template<size_t MM, size_t NN>
+	Matrix<Type, P, Q> operator+(const SliceT<const Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
+	{
+		return Matrix<Type, P, Q> {*this} + other;
+	}
+
+	Matrix<Type, P, Q> operator+(const Matrix<Type, P, Q> &other)
+	{
+		return Matrix<Type, P, Q> {*this} + other;
+	}
+
+	Matrix<Type, P, Q> operator+(const Type &other)
+	{
+		return Matrix<Type, P, Q> {*this} + other;
+	}
+
 	// allow assigning vectors to a slice that are in the axis
 	template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations
 	SliceT<MatrixT, Type, 1, Q, M, N> &operator=(const Vector<Type, Q> &other)
@@ -222,29 +255,19 @@ public:
 		return self;
 	}

-	SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &other)
+	SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &scalar)
 	{
-		return operator*=(Type(1) / other);
+		return operator*=(Type(1) / scalar);
 	}

-	Matrix<Type, P, Q> operator*(const Type &other) const
+	Matrix<Type, P, Q> operator*(const Type &scalar) const
 	{
-		const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
-		Matrix<Type, P, Q> res;
-
-		for (size_t i = 0; i < P; i++) {
-			for (size_t j = 0; j < Q; j++) {
-				res(i, j) = self(i, j) * other;
-			}
-		}
-
-		return res;
+		return Matrix<Type, P, Q> {*this} * scalar;
 	}

-	Matrix<Type, P, Q> operator/(const Type &other) const
+	Matrix<Type, P, Q> operator/(const Type &scalar) const
 	{
-		const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
-		return self * (Type(1) / other);
+		return (*this) * (1 / scalar);
 	}

 	template<size_t R, size_t S>
@@ -317,6 +317,7 @@ public:
 	}
 };

+using SquareMatrix2f = SquareMatrix<float, 2>;
 using SquareMatrix3f = SquareMatrix<float, 3>;
 using SquareMatrix3d = SquareMatrix<double, 3>;

@@ -102,7 +102,6 @@ public:

 };

-
 using Vector2f = Vector2<float>;
 using Vector2d = Vector2<double>;

@@ -262,6 +262,79 @@ TEST(MatrixSliceTest, Slice)
 	float O_check_data_12 [4] = {2.5, 3, 4, 5};
 	EXPECT_EQ(res_12, (SquareMatrix<float, 2>(O_check_data_12)));
 }
+TEST(MatrixSliceTest, SliceAdditions)
+{
+	float data[9] = {0, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	SquareMatrix3f A{data};
+
+	float operand_data [4] = {2, 1,
+				  -3, -1
+				 };
+	const SquareMatrix2f operand(operand_data);
+
+	// 2x2 Slice + 2x2 Matrix
+	SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) + operand;
+	float res_1_check_data[4] = {6, 6,
+				     4, 7
+				    };
+	EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data)));
+
+	// 2x1 Slice + 2x1 Slice
+	Vector2f res_2 = A.slice<2, 1>(1, 1) + operand.slice<2, 1>(0, 0);
+	EXPECT_EQ(res_2, Vector2f(7, 5));
+
+	// 3x3 Slice + Scalar
+	SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) + (-1);
+	float res_3_check_data[9] = {-1, 1, 2,
+				     3, 4, 5,
+				     6, 7, 9
+				    };
+	EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data)));
+
+	// 3x1 Slice + 3 Vector
+	Vector3f res_4 = A.col(1) + Vector3f(1, -2, 3);
+	EXPECT_EQ(res_4, Vector3f(3, 3, 11));
+
+}
+TEST(MatrixSliceTest, SliceSubtractions)
+{
+	float data[9] = {0, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	SquareMatrix3f A{data};
+
+	float operand_data[4] = {2, 1,
+				 -3, -1
+				};
+	const SquareMatrix2f operand(operand_data);
+
+	// 2x2 Slice - 2x2 Matrix
+	SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) - operand;
+	float res_1_check_data[4] = {2, 4,
+				     10, 9
+				    };
+	EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data)));
+
+	// 2x1 Slice - 2x1 Slice
+	Vector2f res_2 = A.slice<2, 1>(1, 1) - operand.slice<2, 1>(0, 0);
+	EXPECT_EQ(res_2, Vector2f(3, 11));
+
+	// 3x3 Slice - Scalar
+	SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) - (-1);
+	float res_3_check_data[9] = {1, 3, 4,
+				     5, 6, 7,
+				     8, 9, 11
+				    };
+	EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data)));
+
+	// 3x1 Slice - 3 Vector
+	Vector3f res_4 = A.col(1) - Vector3f(1, -2, 3);
+	EXPECT_EQ(res_4, Vector3f(1, 7, 5));
+}

 TEST(MatrixSliceTest, XYAssignmentTest)
 {
@@ -80,4 +80,13 @@ TEST(MatrixVector3Test, Vector3)
 	Vector3f m2(3.1f, 4.1f, 5.1f);
 	EXPECT_EQ(m2, m1 + 2.1f);
 	EXPECT_EQ(m2 - 2.1f, m1);
+
+	// Test Addition and Subtraction of Slices
+	Vector3f v1(3, 13, 0);
+	Vector3f v2(42, 6, 256);
+
+	EXPECT_EQ(v1.xy() - v2.xy(), Vector2f(-39, 7));
+	EXPECT_EQ(v1.xy() + v2.xy(), Vector2f(45, 19));
+	EXPECT_EQ(v1.xy() + 2.f, Vector2f(5, 15));
+	EXPECT_EQ(v1.xy() - 2.f, Vector2f(1, 11));
 }
@@ -43,3 +43,4 @@ target_include_directories(motion_planning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

 px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS motion_planning)
 px4_add_unit_gtest(SRC PositionSmoothingTest.cpp LINKLIBS motion_planning)
+px4_add_unit_gtest(SRC HeadingSmoothingTest.cpp LINKLIBS motion_planning)
@@ -35,7 +35,7 @@

 HeadingSmoothing::HeadingSmoothing()
 {
-	_velocity_smoothing.setMaxVel(M_PI_F); // smoothed "velocity" is heading [-pi, pi]
+	_velocity_smoothing.setMaxVel(M_TWOPI_F); // "velocity" is heading. 2Pi limit is needed for correct angle wrapping
 }

 void HeadingSmoothing::reset(const float heading, const float heading_rate)
@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the Heading Smoothing library
+ * Run exclusively this test with the command "make tests TESTFILTER=HeadingSmoothing"
+ */
+
+#include "mathlib/math/Limits.hpp"
+#include <gtest/gtest.h>
+
+#include <motion_planning/HeadingSmoothing.hpp>
+
+using namespace matrix;
+
+class HeadingSmoothingTest : public ::testing::Test
+{
+public:
+	HeadingSmoothingTest()
+	{
+		_smoothing.setMaxHeadingRate(15.f);
+		_smoothing.setMaxHeadingAccel(1.f);
+	}
+
+	HeadingSmoothing _smoothing;
+	float _dt{.1f};
+};
+
+TEST_F(HeadingSmoothingTest, convergence)
+{
+	const float heading_current = math::radians(0.f);
+	const float heading_target = math::radians(5.f);
+	_smoothing.reset(heading_current, 0.f);
+
+	const int nb_steps = ceilf(1.f / _dt);
+
+	for (int i = 0; i < nb_steps; i++) {
+		_smoothing.update(heading_target, _dt);
+	}
+
+	const float heading = _smoothing.getSmoothedHeading();
+	const float heading_rate = _smoothing.getSmoothedHeadingRate();
+	EXPECT_EQ(heading, heading_target) << "heading (deg): " << math::degrees(heading);
+	EXPECT_EQ(heading_rate, 0.f);
+}
+
+TEST_F(HeadingSmoothingTest, zero_crossing)
+{
+	const float heading_current = math::radians(-95.f);
+	const float heading_target = math::radians(5.f);
+	_smoothing.reset(heading_current, 0.f);
+
+	const int nb_steps = ceilf(4.f / _dt);
+
+	for (int i = 0; i < nb_steps; i++) {
+		_smoothing.update(heading_target, _dt);
+	}
+
+	const float heading = _smoothing.getSmoothedHeading();
+	const float heading_rate = _smoothing.getSmoothedHeadingRate();
+	EXPECT_EQ(heading, heading_target) << "heading (deg): " << math::degrees(heading);
+	EXPECT_EQ(heading_rate, 0.f);
+}
+
+TEST_F(HeadingSmoothingTest, wrap_pi)
+{
+	const float heading_current = math::radians(-170.f);
+	const float heading_target = math::radians(170.f);
+	_smoothing.reset(heading_current, 0.f);
+
+	const int nb_steps = ceilf(2.f / _dt);
+
+	for (int i = 0; i < nb_steps; i++) {
+		_smoothing.update(heading_target, _dt);
+	}
+
+	const float heading = _smoothing.getSmoothedHeading();
+	const float heading_rate = _smoothing.getSmoothedHeadingRate();
+	EXPECT_EQ(heading, heading_target) << "heading (deg): " << math::degrees(heading);
+	EXPECT_EQ(heading_rate, 0.f);
+}
+
+TEST_F(HeadingSmoothingTest, positive_rate)
+{
+	const float max_heading_rate = .15f;
+	_smoothing.setMaxHeadingRate(max_heading_rate);
+	const float heading_current = math::radians(-170.f);
+	const float heading_target = math::radians(-20.f);
+	_smoothing.reset(heading_current, 0.f);
+
+	const int nb_steps = ceilf(2.f / _dt);
+
+	for (int i = 0; i < nb_steps; i++) {
+		_smoothing.update(heading_target, _dt);
+	}
+
+	const float heading_rate = _smoothing.getSmoothedHeadingRate();
+	EXPECT_EQ(heading_rate, max_heading_rate);
+}
+
+TEST_F(HeadingSmoothingTest, negative_rate)
+{
+	const float max_heading_rate = 0.15;
+	_smoothing.setMaxHeadingRate(max_heading_rate);
+	const float heading_current = math::radians(-20.f);
+	const float heading_target = math::radians(-140.f);
+	_smoothing.reset(heading_current, 0.f);
+
+	const int nb_steps = ceilf(2.f / _dt);
+
+	for (int i = 0; i < nb_steps; i++) {
+		_smoothing.update(heading_target, _dt);
+	}
+
+	const float heading_rate = _smoothing.getSmoothedHeadingRate();
+	EXPECT_EQ(heading_rate, -max_heading_rate);
+}
@@ -78,18 +78,16 @@ float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2

 	const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned;
 	const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero();
-	const Vector2f distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) *
-			prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
-	const Vector2f curr_pos_to_path = distance_on_line_segment -
-					  prev_wp_to_curr_pos; // Shortest vector from the current position to the path
+	_distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * prev_wp_to_curr_wp_u;
+	_curr_pos_to_path = _distance_on_line_segment - prev_wp_to_curr_pos;

-	if (curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point
-		return atan2f(curr_pos_to_path(1), curr_pos_to_path(0));
+	if (_curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point
+		return atan2f(_curr_pos_to_path(1), _curr_pos_to_path(0));
 	}

-	const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(),
-					  2.f)); // Length of the vector from the endpoint of distance_on_line_segment to the intersection point
-	const Vector2f prev_wp_to_intersection_point = distance_on_line_segment + line_extension *
+	const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(_curr_pos_to_path.norm(),
+					  2.f)); // Length of the vector from the endpoint of _distance_on_line_segment to the intersection point
+	const Vector2f prev_wp_to_intersection_point = _distance_on_line_segment + line_extension *
 			prev_wp_to_curr_wp_u;
 	const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos;
 	return atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0));
@@ -103,6 +103,8 @@ public:
 				 float vehicle_speed);

 	float getLookaheadDistance() {return _lookahead_distance;};
+	float getCrosstrackError() {return _curr_pos_to_path.norm();};
+	float getDistanceOnLineSegment() {return _distance_on_line_segment.norm();};

 protected:
 	/**
@@ -122,5 +124,7 @@ protected:
 		float lookahead_min{1.f};
 	} _params{};
 private:
-	float _lookahead_distance{0.f};
+	float _lookahead_distance{0.f}; // Radius of the circle around the vehicle
+	Vector2f _distance_on_line_segment{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
+	Vector2f _curr_pos_to_path{}; // Shortest vector from the current position to the path
 };
@@ -44,11 +44,11 @@

 #include "matrix/Matrix.hpp"
 #include "matrix/Vector2.hpp"
+#include <mathlib/math/Functions.hpp>

 using math::constrain;
 using math::max;
 using math::min;
-using namespace time_literals;

 static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);}

@@ -525,16 +525,12 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec

 	if (1.f - param.fast_descend < FLT_EPSILON) {
 		// During fast descend, we control airspeed over the pitch control loop. Give minimal thrust as soon as we are descending
-		if (specific_energy_rates.spe_rate.estimate > 0) { // We have a positive altitude rate and are stil climbing
-			throttle_setpoint = param.throttle_trim; // Do not cut off throttle yet
-
-		} else {
-			throttle_setpoint = param.throttle_min;
-		}
+		throttle_setpoint = param.throttle_min;

 	} else {
 		_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
-		throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag);
+		throttle_setpoint = (1.f - param.fast_descend) * _calcThrottleControlOutput(limit, ste_rate, param,
+				    flag) + param.fast_descend * param.throttle_min;
 	}

 	// Rate limit the throttle demand
@@ -677,6 +673,11 @@ void TECS::initControlParams(float target_climbrate, float target_sinkrate, floa
 	_control_param.throttle_min = throttle_min;
 }

+float TECS::calcTrueAirspeedSetpoint(float eas_to_tas, float eas_setpoint)
+{
+	return lerp(eas_to_tas * eas_setpoint, _control_param.tas_max, _fast_descend);
+}
+
 void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed,
 		      float eas_to_tas)
 {
@@ -699,6 +700,9 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 						.tas_rate = 0.0f};

 	_control.initialize(control_setpoint, control_input, _control_param, _control_flag);
+
+	_fast_descend = 0.f;
+	_enabled_fast_descend_timestamp = 0U;
 }

 void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
@@ -753,8 +757,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		TECSControl::Setpoint control_setpoint;
 		control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();
 		control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect();
-		control_setpoint.tas_setpoint = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas *
-						EAS_setpoint;
+		control_setpoint.tas_setpoint = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint);

 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,
@@ -765,11 +768,13 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	}

 	_debug_status.control = _control.getDebugOutput();
+	_debug_status.true_airspeed_sp = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint);
 	_debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed;
 	_debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate;
 	_debug_status.altitude_reference = _altitude_reference_model.getAltitudeReference().alt;
 	_debug_status.height_rate_reference = _altitude_reference_model.getAltitudeReference().alt_rate;
 	_debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect();
+	_debug_status.fast_descend = _fast_descend;

 	_update_timestamp = now;
 }
@@ -778,13 +783,22 @@ void TECS::_setFastDescend(const float alt_setpoint, const float alt)
 {
 	if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON)
 	    && ((alt_setpoint + _fast_descend_alt_err) < alt)) {
-		_fast_descend = 1.f;
+		auto now = hrt_absolute_time();
+
+		if (_enabled_fast_descend_timestamp == 0U) {
+			_enabled_fast_descend_timestamp = now;
+		}
+
+		_fast_descend = constrain(max(_fast_descend, static_cast<float>(now - _enabled_fast_descend_timestamp) /
+					      static_cast<float>(FAST_DESCEND_RAMP_UP_TIME)), 0.f, 1.f);

 	} else if ((_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) {
 		// Were in fast descend, scale it down. up until 5m above target altitude
 		_fast_descend = constrain((alt - alt_setpoint - 5.f) / _fast_descend_alt_err, 0.f, 1.f);
+		_enabled_fast_descend_timestamp = 0U;

 	} else {
 		_fast_descend = 0.f;
+		_enabled_fast_descend_timestamp = 0U;
 	}
 }
@@ -50,6 +50,8 @@
 #include <motion_planning/VelocitySmoothing.hpp>
 #include <motion_planning/ManualVelocitySmoothingZ.hpp>

+using namespace time_literals;
+
 class TECSAirspeedFilter
 {
 public:
@@ -203,8 +205,8 @@ public:
 		float equivalent_airspeed_trim;		///< Equivalent cruise airspeed for airspeed less mode [m/s].
 		float tas_min;				///< True airspeed demand lower limit [m/s].
 		float tas_max;				///< True airspeed demand upper limit [m/s].
-		float pitch_max;			///< Maximum pitch angle allowed in [rad].
-		float pitch_min;			///< Minimal pitch angle allowed in [rad].
+		float pitch_max;			///< Maximum pitch angle above trim allowed in [rad].
+		float pitch_min;			///< Minimal pitch angle below trim allowed in [rad].
 		float throttle_trim;		///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1]
 		float throttle_max;			///< Normalized throttle upper limit.
 		float throttle_min;			///< Normalized throttle lower limit.
@@ -320,7 +322,7 @@ public:
 	/**
 	 * @brief Get the pitch setpoint.
 	 *
-	 * @return THe commanded pitch angle in [rad].
+	 * @return The commanded pitch angle above trim in [rad].
 	 */
 	float getPitchSetpoint() const {return _pitch_setpoint;};
 	/**
@@ -478,7 +480,7 @@ private:
 	 * @param seb_rate is the specific energy balance rate in [m²/s³].
 	 * @param param is the control parameters.
 	 * @param flag is the control flags.
-	 * @return pitch setpoint angle [rad].
+	 * @return pitch setpoint angle above trim [rad].
 	 */
 	float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
 				      const Flag &flag) const;
@@ -537,7 +539,7 @@ private:

 	// Output
 	DebugOutput _debug_output;				///< Debug output.
-	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint [rad].
+	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint above trim [rad].
 	float _throttle_setpoint{0.0f};				///< Controlled throttle setpoint [0,1].
 	float _ratio_undersped{0.0f};				///< A continuous representation of how "undersped" the TAS is [0,1]
 };
@@ -547,11 +549,13 @@ class TECS
 public:
 	struct DebugOutput {
 		TECSControl::DebugOutput control;
+		float true_airspeed_sp;
 		float true_airspeed_filtered;
 		float true_airspeed_derivative;
 		float altitude_reference;
 		float height_rate_reference;
 		float height_rate_direct;
+		float fast_descend;
 	};
 public:
 	TECS() = default;
@@ -658,6 +662,17 @@ private:
 	void initControlParams(float target_climbrate, float target_sinkrate, float eas_to_tas, float pitch_limit_max,
 			       float pitch_limit_min, float throttle_min, float throttle_setpoint_max, float throttle_trim);

+	/**
+	 * @brief calculate true airspeed setpoint
+	 *
+	 * Calculate true airspeed setpoint based on input and fast descend ratio
+	 *
+	 * @param eas_to_tas is the eas to tas conversion factor
+	 * @param eas_setpoint is the desired equivalent airspeed setpoint [m/s]
+	 * @return true airspeed setpoint[m/s]
+	 */
+	float calcTrueAirspeedSetpoint(float eas_to_tas, float eas_setpoint);
+
 	/**
 	 * @brief Initialize the control loop
 	 *
@@ -675,9 +690,11 @@ private:
 	float _equivalent_airspeed_max{20.0f};				///< equivalent airspeed demand upper limit (m/sec)
 	float _fast_descend_alt_err{-1.f};	 				///< Altitude difference between current altitude to altitude setpoint needed to descend with higher airspeed [m].
 	float _fast_descend{0.f};					///< Value for fast descend in [0,1]. continuous value used to flatten the high speed value out when close to target altitude.
+	hrt_abstime _enabled_fast_descend_timestamp{0U};		///< timestamp at activation of fast descend mode

 	static constexpr float DT_MIN = 0.001f;				///< minimum allowed value of _dt (sec)
 	static constexpr float DT_MAX = 1.0f;				///< max value of _dt allowed before a filter state reset is performed (sec)
+	static constexpr hrt_abstime FAST_DESCEND_RAMP_UP_TIME = 2_s; 	///< Ramp up time until fast descend is fully engaged

 	DebugOutput _debug_status{};

@@ -571,7 +571,12 @@ void EKF2::Run()
 				const float wind_direction_accuracy_rad = math::radians(vehicle_command.param4);
 				_ekf.resetWindToExternalObservation(vehicle_command.param1, wind_direction_rad, vehicle_command.param2,
 								    wind_direction_accuracy_rad);
+				command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+#else
+				command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 #endif // CONFIG_EKF2_WIND
+				command_ack.timestamp = hrt_absolute_time();
+				_vehicle_command_ack_pub.publish(command_ack);
 			}
 		}
 	}
@@ -181,6 +181,7 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
 	_initial_heading = _yaw;
 	_slew_rate_yaw.setForcedValue(_yaw);
 	_slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get()));
+	_slew_rate_velocity.setSlewRate(_param_mpc_acc_hor.get());

 	// need a valid position and velocity
 	ret = ret && _position.isAllFinite() && _velocity.isAllFinite();
@@ -216,6 +217,7 @@ bool FlightTaskOrbit::update()
 	if (_is_position_on_circle()) {
 		if (_in_circle_approach) {
 			_in_circle_approach = false;
+			_slew_rate_velocity.setForcedValue(0.f); // reset the slew rate when moving between orbits.
 			FlightTaskManualAltitudeSmoothVel::_smoothing.reset(
 				PX4_ISFINITE(_acceleration_setpoint(2)) ? _acceleration_setpoint(2) : 0.f,
 				PX4_ISFINITE(_velocity_setpoint(2)) ? _velocity_setpoint(2) : _velocity(2),
@@ -329,15 +331,20 @@ void FlightTaskOrbit::_generate_circle_setpoints()
 	Vector3f center_to_position = _position - _center;
 	// xy velocity to go around in a circle
 	Vector2f velocity_xy(-center_to_position(1), center_to_position(0));
+
+	// slew rate is used to reduce the jerk when starting an orbit.
+	_slew_rate_velocity.update(_orbit_velocity, _deltatime);
+
 	velocity_xy = velocity_xy.unit_or_zero();
-	velocity_xy *= _orbit_velocity;
+	velocity_xy *= _slew_rate_velocity.getState();

 	// xy velocity adjustment to stay on the radius distance
 	velocity_xy += (_orbit_radius - center_to_position.xy().norm()) * Vector2f(center_to_position).unit_or_zero();

 	_position_setpoint(0) = _position_setpoint(1) = NAN;
 	_velocity_setpoint.xy() = velocity_xy;
-	_acceleration_setpoint.xy() = -Vector2f(center_to_position.unit_or_zero()) * _orbit_velocity * _orbit_velocity /
+	_acceleration_setpoint.xy() = -Vector2f(center_to_position.unit_or_zero()) * _slew_rate_velocity.getState() *
+				      _slew_rate_velocity.getState() /
 				      _orbit_radius;
 }

@@ -47,6 +47,7 @@
 #include <lib/slew_rate/SlewRateYaw.hpp>
 #include <lib/motion_planning/PositionSmoothing.hpp>
 #include <lib/motion_planning/VelocitySmoothing.hpp>
+#include <lib/slew_rate/SlewRate.hpp>


 class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel
@@ -127,6 +128,7 @@ private:
 	bool _currently_orbiting{false};
 	float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */
 	SlewRateYaw<float> _slew_rate_yaw;
+	SlewRate<float> _slew_rate_velocity;

 	orb_advert_t _mavlink_log_pub{nullptr};
 	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
@@ -424,7 +424,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
 	tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control;
 	tecs_status.height_rate = -_local_pos.vz;
 	tecs_status.equivalent_airspeed_sp = equivalent_airspeed_sp;
-	tecs_status.true_airspeed_sp = _eas2tas * equivalent_airspeed_sp;
+	tecs_status.true_airspeed_sp = debug_output.true_airspeed_sp;
 	tecs_status.true_airspeed_filtered = debug_output.true_airspeed_filtered;
 	tecs_status.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
 	tecs_status.true_airspeed_derivative = debug_output.true_airspeed_derivative;
@@ -439,6 +439,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
 	tecs_status.pitch_sp_rad = _tecs.get_pitch_setpoint();
 	tecs_status.throttle_trim = throttle_trim;
 	tecs_status.underspeed_ratio = _tecs.get_underspeed_ratio();
+	tecs_status.fast_descend_ratio = debug_output.fast_descend;

 	tecs_status.timestamp = hrt_absolute_time();

@@ -1029,11 +1030,15 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 					   _current_latitude, _current_longitude, _current_altitude,
 					   &dist_xy, &dist_z);

+		const float acc_rad_z = (PX4_ISFINITE(pos_sp_curr.alt_acceptance_radius)
+					 && pos_sp_curr.alt_acceptance_radius > FLT_EPSILON) ? pos_sp_curr.alt_acceptance_radius :
+					_param_nav_fw_alt_rad.get();
+
 		// Achieve position setpoint altitude via loiter when laterally close to WP.
 		// Detect if system has switchted into a Loiter before (check _position_sp_type), and in that
 		// case remove the dist_xy check (not switch out of Loiter until altitude is reached).
 		if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f)
-		    && (dist_z > _param_nav_fw_alt_rad.get())
+		    && (dist_z > acc_rad_z)
 		    && (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {

 			// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
@@ -55,12 +55,13 @@ bool RoverLandDetector::_get_landed_state()
 	const float distance_to_home = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
 				       _home_position(0), _home_position(1));

-	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
+	    || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
 		return true; // If Landing has been requested then say we have landed.

 	} else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
 		   && distance_to_home < _param_nav_acc_rad.get() && _param_rtl_land_delay.get() > -FLT_EPSILON) {
-		return true; // If the rover reaches the home position during RTL we say we have landed
+		return true; // If the rover reaches the home position during RTL we say we have landed.

 	} else {
 		return !_armed;  // If we are armed we are not landed.
@@ -58,6 +58,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("commander_state");
 	add_topic("config_overrides");
 	add_topic("cpuload");
+	add_topic("distance_sensor_mode_change_request");
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");
 	add_optional_topic("external_ins_local_position");
@@ -91,41 +91,6 @@ Mission::on_activation()
 	MissionBase::on_activation();
 }

-bool
-Mission::isLanding()
-{
-	if (get_land_start_available()) {
-		static constexpr size_t max_num_next_items{1u};
-		int32_t next_mission_items_index[max_num_next_items];
-		size_t num_found_items;
-
-		getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items);
-
-		// vehicle is currently landing if
-		//  mission valid, still flying, and in the landing portion of mission (past land start marker)
-		bool on_landing_stage = (num_found_items > 0U) &&  _mission.current_seq > next_mission_items_index[0U];
-
-		// special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the
-		// distance to the WP is below the loiter radius + acceptance.
-		if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U]
-		    && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
-			const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-						_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-
-			// consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case.
-			const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius)
-					&& fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) :
-					_navigator->get_loiter_radius();
-
-			on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs);
-		}
-
-		return _navigator->get_mission_result()->valid && on_landing_stage;
-
-	} else {
-		return false;
-	}
-}

 bool
 Mission::set_current_mission_index(uint16_t index)
@@ -244,6 +209,13 @@ void Mission::setActiveMissionItems()
 			mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		}

+		// prevent fixed wing lateral guidance from loitering at a waypoint as part of a mission landing if the altitude
+		// is not achieved.
+		if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() &&
+		    _mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
+			pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
+		}
+
 		// Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout
 		// This is done by setting the position triplet's next position's valid flag to false,
 		// which makes the FlightTask disregard the next position
@@ -67,7 +67,6 @@ public:
 	uint16_t get_land_start_index() const { return _mission.land_start_index; }
 	bool get_land_start_available() const { return hasMissionLandStart(); }

-	bool isLanding();

 private:

@@ -367,6 +367,42 @@ MissionBase::on_active()
 	updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item);
 }

+bool
+MissionBase::isLanding()
+{
+	if (hasMissionLandStart() && (_mission.current_seq > _mission.land_start_index)) {
+		static constexpr size_t max_num_next_items{1u};
+		int32_t next_mission_items_index[max_num_next_items];
+		size_t num_found_items;
+
+		getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items);
+
+		// vehicle is currently landing if
+		//  mission valid, still flying, and in the landing portion of mission (past land start marker)
+		bool on_landing_stage = (num_found_items > 0U) &&  _mission.current_seq > next_mission_items_index[0U];
+
+		// special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the
+		// distance to the WP is below the loiter radius + acceptance.
+		if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U]
+		    && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
+			const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
+						_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+
+			// consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case.
+			const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius)
+					&& fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) :
+					_navigator->get_loiter_radius();
+
+			on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs);
+		}
+
+		return _navigator->get_mission_result()->valid && on_landing_stage;
+
+	} else {
+		return false;
+	}
+}
+
 void MissionBase::update_mission()
 {
 	if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !isMissionValid()) {
@@ -73,6 +73,8 @@ public:
 	virtual void on_activation() override;
 	virtual void on_active() override;

+	virtual bool isLanding();
+
 protected:

 	/**
@@ -321,6 +323,7 @@ protected:
 	 */
 	void setMissionIndex(int32_t index);

+
 	bool _is_current_planned_mission_item_valid{false};	/**< Flag indicating if the currently loaded mission item is valid*/
 	bool _mission_has_been_activated{false};		/**< Flag indicating if the mission has been activated*/
 	bool _mission_checked{false};				/**< Flag indicating if the mission has been checked by the mission validator*/
@@ -673,6 +673,10 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 		sp->acceptance_radius = _navigator->get_default_acceptance_radius();
 	}

+	// by default, FW guidance logic will take alt acceptance from NAV_FW_ALT_RAD, in some special cases
+	// we override it after this
+	sp->alt_acceptance_radius = NAN;
+
 	sp->cruising_speed = _navigator->get_cruising_speed();
 	sp->cruising_throttle = _navigator->get_cruising_throttle();

@@ -1026,6 +1030,7 @@ void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_i

 	if (_navigator->get_nav_min_gnd_dist_param() > FLT_EPSILON && _mission_item.nav_cmd != NAV_CMD_LAND
 	    && _mission_item.nav_cmd != NAV_CMD_VTOL_LAND && _mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION
+	    && _mission_item.nav_cmd != NAV_CMD_IDLE
 	    && _navigator->get_local_position()->dist_bottom_valid
 	    && _navigator->get_local_position()->dist_bottom < _navigator->get_nav_min_gnd_dist_param()
 	    && _navigator->get_local_position()->vz > FLT_EPSILON
@@ -64,6 +64,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/distance_sensor_mode_change_request.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/gimbal_manager_set_attitude.h>
 #include <uORB/topics/home_position.h>
@@ -316,6 +317,7 @@ private:
 	uORB::Publication<vehicle_command_s>		_vehicle_cmd_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_roi_s>		_vehicle_roi_pub{ORB_ID(vehicle_roi)};
 	uORB::Publication<mode_completed_s> _mode_completed_pub{ORB_ID(mode_completed)};
+	uORB::PublicationData<distance_sensor_mode_change_request_s> _distance_sensor_mode_change_request_pub{ORB_ID(distance_sensor_mode_change_request)};

 	orb_advert_t	_mavlink_log_pub{nullptr};	/**< the uORB advert to send messages over mavlink */

@@ -336,6 +338,7 @@ private:
 	position_setpoint_triplet_s			_takeoff_triplet{};	/**< triplet for non-mission direct takeoff command */
 	vehicle_roi_s					_vroi{};		/**< vehicle ROI */

+
 	perf_counter_t	_loop_perf;			/**< loop performance counter */

 	Geofence	_geofence;			/**< class that handles the geofence */
@@ -400,6 +403,8 @@ private:

 	void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);

+	void publish_distance_sensor_mode_request();
+
 	bool geofence_allows_position(const vehicle_global_position_s &pos);

 	DEFINE_PARAMETERS(
@@ -110,6 +110,11 @@ Navigator::Navigator() :
 	_mission_sub = orb_subscribe(ORB_ID(mission));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));

+	_distance_sensor_mode_change_request_pub.advertise();
+	_distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time();
+	_distance_sensor_mode_change_request_pub.get().request_on_off = distance_sensor_mode_change_request_s::REQUEST_OFF;
+	_distance_sensor_mode_change_request_pub.update();
+
 	// Update the timeout used in mission_block (which can't hold it's own parameters)
 	_mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get());

@@ -898,6 +903,8 @@ void Navigator::run()

 		publish_navigator_status();

+		publish_distance_sensor_mode_request();
+
 		_geofence.run();

 		perf_end(_loop_perf);
@@ -1116,9 +1123,14 @@ float Navigator::get_default_acceptance_radius()
 float Navigator::get_altitude_acceptance_radius()
 {
 	if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+
+		const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current;
 		const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next;

-		if (!force_vtol() && next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
+		if ((PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON)) {
+			return curr_sp.alt_acceptance_radius;
+
+		} else if (!force_vtol() && next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
 			// Use separate (tighter) altitude acceptance for clean altitude starting point before FW landing
 			return _param_nav_fw_altl_rad.get();

@@ -1442,6 +1454,30 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
 	_vehicle_cmd_pub.publish(*vcmd);
 }

+void Navigator::publish_distance_sensor_mode_request()
+{
+	// Send request to enable distance sensor when in the landing phase of a mission or RTL
+	if (((_navigation_mode == &_rtl) && _rtl.isLanding()) || ((_navigation_mode == &_mission) && _mission.isLanding())) {
+
+		if (_distance_sensor_mode_change_request_pub.get().request_on_off !=
+		    distance_sensor_mode_change_request_s::REQUEST_ON) {
+
+			_distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time();
+			_distance_sensor_mode_change_request_pub.get().request_on_off =
+				distance_sensor_mode_change_request_s::REQUEST_ON;
+			_distance_sensor_mode_change_request_pub.update();
+		}
+
+	} else if (_distance_sensor_mode_change_request_pub.get().request_on_off !=
+		   distance_sensor_mode_change_request_s::REQUEST_OFF) {
+
+		_distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time();
+		_distance_sensor_mode_change_request_pub.get().request_on_off =
+			distance_sensor_mode_change_request_s::REQUEST_OFF;
+		_distance_sensor_mode_change_request_pub.update();
+	}
+}
+
 void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result)
 {
 	vehicle_command_ack_s command_ack = {};
@@ -304,6 +304,28 @@ void RTL::on_active()
 	}
 }

+bool RTL::isLanding()
+{
+	bool is_landing{false};
+
+	switch (_rtl_type) {
+	case RtlType::RTL_MISSION_FAST:
+	case RtlType::RTL_MISSION_FAST_REVERSE:
+	case RtlType::RTL_DIRECT_MISSION_LAND:
+		is_landing = _rtl_mission_type_handle->isLanding();
+		break;
+
+	case RtlType::RTL_DIRECT:
+		is_landing = _rtl_direct.isLanding();
+		break;
+
+	default:
+		break;
+	}
+
+	return is_landing;
+}
+
 void RTL::setRtlTypeAndDestination()
 {

@@ -89,6 +89,8 @@ public:

 	void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; }

+	bool isLanding();
+
 private:
 	enum class DestinationType {
 		DESTINATION_TYPE_HOME,
@@ -101,10 +101,11 @@ void RtlDirect::on_active()
 	parameters_update();

 	if (_rtl_state != RTLState::IDLE && is_mission_item_reached_or_completed()) {
+		_updateRtlState();
 		set_rtl_item();
 	}

-	if (_rtl_state != RTLState::IDLE) { //TODO: rename _rtl_state to _rtl_state_next (when in IDLE we're actually in LAND)
+	if (_rtl_state != RTLState::IDLE && _rtl_state != RTLState::LAND) {
 		//check for terrain collision and update altitude if needed
 		// note: it may trigger multiple times during a RTL, as every time the altitude set is reset
 		updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item);
@@ -154,6 +155,61 @@ void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s
 	}
 }

+void RtlDirect::_updateRtlState()
+{
+	RTLState new_state{RTLState::IDLE};
+
+	switch (_rtl_state) {
+	case RTLState::CLIMBING:
+		new_state = RTLState::MOVE_TO_LOITER;
+		break;
+
+	case RTLState::MOVE_TO_LOITER:
+		new_state = RTLState::LOITER_DOWN;
+		break;
+
+	case RTLState::LOITER_DOWN:
+		new_state = RTLState::LOITER_HOLD;
+		break;
+
+	case RTLState::LOITER_HOLD:
+		if (_vehicle_status_sub.get().is_vtol
+		    && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			new_state = RTLState::MOVE_TO_LAND;
+
+		} else {
+			new_state = RTLState::MOVE_TO_LAND_HOVER;
+		}
+
+		break;
+
+	case RTLState::MOVE_TO_LAND:
+		new_state = RTLState::TRANSITION_TO_MC;
+		break;
+
+	case RTLState::TRANSITION_TO_MC:
+		new_state = RTLState::MOVE_TO_LAND_HOVER;
+		break;
+
+	case RTLState::MOVE_TO_LAND_HOVER:
+		new_state = RTLState::LAND;
+		break;
+
+	case RTLState::LAND:
+		new_state = RTLState::IDLE;
+		break;
+
+	case RTLState::IDLE: // Fallthrough
+	default:
+		new_state = RTLState::IDLE;
+		break;
+	}
+
+	_rtl_state = new_state;
+
+}
+
+
 void RtlDirect::set_rtl_item()
 {
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -174,7 +230,6 @@ void RtlDirect::set_rtl_item()
 			};
 			setLoiterToAltMissionItem(_mission_item, pos_yaw_sp, _navigator->get_loiter_radius());

-			_rtl_state = RTLState::MOVE_TO_LOITER;
 			break;
 		}

@@ -197,8 +252,6 @@ void RtlDirect::set_rtl_item()
 				setMoveToPositionMissionItem(_mission_item, pos_yaw_sp);
 			}

-			_rtl_state = RTLState::LOITER_DOWN;
-
 			break;
 		}

@@ -224,8 +277,6 @@ void RtlDirect::set_rtl_item()
 			// Disable previous setpoint to prevent drift.
 			pos_sp_triplet->previous.valid = false;

-			_rtl_state = RTLState::LOITER_HOLD;
-
 			break;
 		}

@@ -244,14 +295,6 @@ void RtlDirect::set_rtl_item()
 				events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering");
 			}

-			if (_vehicle_status_sub.get().is_vtol
-			    && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-				_rtl_state = RTLState::MOVE_TO_LAND;
-
-			} else {
-				_rtl_state = RTLState::MOVE_TO_LAND_HOVER;
-			}
-
 			break;
 		}

@@ -274,16 +317,12 @@ void RtlDirect::set_rtl_item()
 			pos_sp_triplet->previous.alt = get_absolute_altitude_for_item(_mission_item);
 			pos_sp_triplet->previous.valid = true;

-			_rtl_state = RTLState::TRANSITION_TO_MC;
-
 			break;
 		}

 	case RTLState::TRANSITION_TO_MC: {
 			set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);

-			_rtl_state = RTLState::MOVE_TO_LAND_HOVER;
-
 			break;
 		}

@@ -295,8 +334,6 @@ void RtlDirect::set_rtl_item()
 			setMoveToPositionMissionItem(_mission_item, pos_yaw_sp);
 			_navigator->reset_position_setpoint(pos_sp_triplet->previous);

-			_rtl_state = RTLState::LAND;
-
 			break;
 		}

@@ -309,8 +346,6 @@ void RtlDirect::set_rtl_item()

 			startPrecLand(_mission_item.land_precision);

-			_rtl_state = RTLState::IDLE;
-
 			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t");
 			events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination");
 			break;
@@ -103,6 +103,8 @@ public:

 	void setRtlPosition(PositionYawSetpoint position, loiter_point_s loiter_pos);

+	bool isLanding() { return (_rtl_state != RTLState::IDLE) && (_rtl_state >= RTLState::LOITER_DOWN);};
+
 private:
 	/**
 	 * @brief Return to launch state machine.
@@ -121,6 +123,12 @@ private:
 	} _rtl_state{RTLState::IDLE}; /*< Current state in the state machine.*/

 private:
+	/**
+	 * @brief Update the RTL state machine.
+	 *
+	 */
+	void _updateRtlState();
+
 	/**
 	 * @brief Set the return to launch control setpoint.
 	 *
@@ -212,6 +212,13 @@ void RtlDirectMissionLand::setActiveMissionItems()
 		}

 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+		// prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude
+		// is not achieved.
+		if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && MissionBase::isLanding()
+		    && _mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
+			pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
+		}
 	}

 	issue_command(_mission_item);
@@ -168,6 +168,11 @@ void RtlMissionFast::setActiveMissionItems()
 		}

 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+		if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() &&
+		    _mission_item.nav_cmd == NAV_CMD_WAYPOINT) {
+			pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
+		}
 	}

 	issue_command(_mission_item);
@@ -60,6 +60,12 @@ void RtlMissionFastReverse::on_inactive()
 				   _mission.current_seq : -1;
 }

+void RtlMissionFastReverse::on_inactivation()
+{
+	MissionBase::on_inactivation();
+	_in_landing_phase = false;
+}
+
 void RtlMissionFastReverse::on_activation()
 {
 	_home_pos_sub.update();
@@ -120,6 +126,7 @@ void RtlMissionFastReverse::setActiveMissionItems()
 		    _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
 		    num_found_items == 0) {
 			handleLanding(new_work_item_type);
+			_in_landing_phase = true;

 		} else {
 			// convert mission item to a simple waypoint, keep loiter to alt
@@ -131,6 +138,8 @@ void RtlMissionFastReverse::setActiveMissionItems()
 			_mission_item.time_inside = 0.0f;

 			pos_sp_triplet->previous = pos_sp_triplet->current;
+
+			_in_landing_phase = false;
 		}

 		if (num_found_items > 0) {
@@ -58,6 +58,9 @@ public:
 	void on_activation() override;
 	void on_active() override;
 	void on_inactive() override;
+	void on_inactivation() override;
+
+	bool isLanding() override {return _in_landing_phase;};

 	rtl_time_estimate_s calc_rtl_time_estimate() override;

@@ -68,5 +71,7 @@ private:

 	int _mission_index_prior_rtl{-1};

+	bool _in_landing_phase{false};
+
 	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
 };
@@ -99,12 +99,94 @@ void RoverDifferential::Run()

 		} break;

+	case vehicle_status_s::NAVIGATION_STATE_STAB: {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_differential_setpoint_s rover_differential_setpoint{};
+				rover_differential_setpoint.timestamp = timestamp;
+				rover_differential_setpoint.forward_speed_setpoint = NAN;
+				rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
+						STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
+				rover_differential_setpoint.yaw_setpoint = NAN;
+
+				if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
+				    || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control
+					_yaw_ctl = false;
+
+
+				} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
+					if (!_yaw_ctl) {
+						_stab_desired_yaw = _vehicle_yaw;
+						_yaw_ctl = true;
+					}
+
+					rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw;
+					rover_differential_setpoint.yaw_rate_setpoint = NAN;
+
+				}
+
+				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
+			}
+
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL: {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_differential_setpoint_s rover_differential_setpoint{};
+				rover_differential_setpoint.timestamp = timestamp;
+				rover_differential_setpoint.forward_speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
+						-1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get());
+				rover_differential_setpoint.forward_speed_setpoint_normalized = NAN;
+				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
+						STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
+				rover_differential_setpoint.yaw_setpoint = NAN;
+
+				if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
+				    || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
+					_yaw_ctl = false;
+
+
+				} else { // Course control if the yaw rate input is zero (keep driving on a straight line)
+					if (!_yaw_ctl) {
+						_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
+						_pos_ctl_start_position_ned = _curr_pos_ned;
+						_yaw_ctl = true;
+					}
+
+					// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
+					const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
+										2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
+					const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(
+							rover_differential_setpoint.forward_speed_setpoint) *
+									     vector_scaling * _pos_ctl_course_direction;
+					// Calculate yaw setpoint
+					const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned,
+								   _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed));
+					rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ?
+							yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards
+					rover_differential_setpoint.yaw_rate_setpoint = NAN;
+
+				}
+
+				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
+			}
+
+		} break;
+
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 		_rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state);
 		break;

 	default: // Unimplemented nav states will stop the rover
+		_rover_differential_control.resetControllers();
+		_yaw_ctl = false;
 		rover_differential_setpoint_s rover_differential_setpoint{};
 		rover_differential_setpoint.forward_speed_setpoint = NAN;
 		rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f;
@@ -115,8 +197,9 @@ void RoverDifferential::Run()
 		break;
 	}

-	if (!_armed) { // Reset on disarm
+	if (!_armed) { // Reset when disarmed
 		_rover_differential_control.resetControllers();
+		_yaw_ctl = false;
 	}

 	_rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed);
@@ -135,6 +218,12 @@ void RoverDifferential::updateSubscriptions()
 	if (_vehicle_status_sub.updated()) {
 		vehicle_status_s vehicle_status{};
 		_vehicle_status_sub.copy(&vehicle_status);
+
+		if (vehicle_status.nav_state != _nav_state) { // Reset on mode change
+			_rover_differential_control.resetControllers();
+			_yaw_ctl = false;
+		}
+
 		_nav_state = vehicle_status.nav_state;
 		_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
 	}
@@ -142,7 +231,7 @@ void RoverDifferential::updateSubscriptions()
 	if (_vehicle_angular_velocity_sub.updated()) {
 		vehicle_angular_velocity_s vehicle_angular_velocity{};
 		_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
-		_vehicle_yaw_rate = vehicle_angular_velocity.xyz[2];
+		_vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f;
 	}

 	if (_vehicle_attitude_sub.updated()) {
@@ -155,9 +244,10 @@ void RoverDifferential::updateSubscriptions()
 	if (_vehicle_local_position_sub.updated()) {
 		vehicle_local_position_s vehicle_local_position{};
 		_vehicle_local_position_sub.copy(&vehicle_local_position);
+		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
 		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
 		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
-		_vehicle_forward_speed = velocity_in_body_frame(0);
+		_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
 	}
 }

@@ -39,6 +39,7 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/pure_pursuit/PurePursuit.hpp>

 // uORB includes
 #include <uORB/Publication.hpp>
@@ -60,6 +61,14 @@

 using namespace time_literals;

+// Constants
+static constexpr float STICK_DEADZONE =
+	0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
+static constexpr float YAW_RATE_THRESHOLD =
+	0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero
+static constexpr float SPEED_THRESHOLD =
+	0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero
+
 class RoverDifferential : public ModuleBase<RoverDifferential>, public ModuleParams,
 	public px4::ScheduledWorkItem
 {
@@ -103,8 +112,10 @@ private:
 	// Instances
 	RoverDifferentialGuidance _rover_differential_guidance{this};
 	RoverDifferentialControl _rover_differential_control{this};
+	PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library

 	// Variables
+	Vector2f _curr_pos_ned{};
 	matrix::Quatf _vehicle_attitude_quaternion{};
 	float _vehicle_yaw_rate{0.f};
 	float _vehicle_forward_speed{0.f};
@@ -112,9 +123,15 @@ private:
 	float _max_yaw_rate{0.f};
 	int _nav_state{0};
 	bool _armed{false};
+	bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode
+	float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode
+	Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode
+	Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode

 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RD_MAN_YAW_SCALE>) _param_rd_man_yaw_scale,
-		(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate
+		(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
+		(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
+		(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
 	)
 };
@@ -84,68 +84,59 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con

 	// Closed loop yaw control (Overrides yaw rate setpoint)
 	if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) {
-		if (fabsf(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw) < FLT_EPSILON) {
-			_rover_differential_setpoint.yaw_rate_setpoint = 0.f;
-			pid_reset_integral(&_pid_yaw);
-
-		} else {
-			const float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw);
-			_rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt);
-		}
+		float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw);
+		_rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt);
 	}

 	// Yaw rate control
 	float speed_diff_normalized{0.f};

 	if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
-		if (fabsf(_rover_differential_setpoint.yaw_rate_setpoint) < FLT_EPSILON) {
-			speed_diff_normalized = 0.f;
-			pid_reset_integral(&_pid_yaw_rate);
-
-		} else {
-			const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _param_rd_wheel_track.get(); // Feedforward
-			speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rd_max_speed.get(),
-						_param_rd_max_speed.get(), -1.f, 1.f);
-			speed_diff_normalized = math::constrain(speed_diff_normalized +
-								pid_calculate(&_pid_yaw_rate, _rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0, dt),
-								-1.f, 1.f); // Feedback
+		if (_param_rd_wheel_track.get() > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward
+			const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _param_rd_wheel_track.get() /
+						 2.f;
+			speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rd_max_thr_yaw_r.get(),
+						_param_rd_max_thr_yaw_r.get(), -1.f, 1.f);
 		}

+		speed_diff_normalized = math::constrain(speed_diff_normalized +
+							pid_calculate(&_pid_yaw_rate, _rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0, dt),
+							-1.f, 1.f); // Feedback
+
 	} else { // Use normalized setpoint
 		speed_diff_normalized = PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint_normalized) ?
 					math::constrain(_rover_differential_setpoint.yaw_rate_setpoint_normalized, -1.f, 1.f) : 0.f;
 	}

 	// Speed control
-	float throttle{0.f};
+	float forward_speed_normalized{0.f};

 	if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control
-		if (fabsf(_rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) {
-			pid_reset_integral(&_pid_throttle);
-
-		} else {
-			throttle = pid_calculate(&_pid_throttle, _rover_differential_setpoint.forward_speed_setpoint, vehicle_forward_speed, 0,
-						 dt);
-
-			if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term
-				throttle += math::interpolate<float>(_rover_differential_setpoint.forward_speed_setpoint,
-								     0.f, _param_rd_max_speed.get(),
-								     0.f, 1.f);
-			}
+		if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
+			forward_speed_normalized = math::interpolate<float>(_rover_differential_setpoint.forward_speed_setpoint,
+						   -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(),
+						   -1.f, 1.f);
 		}

+		forward_speed_normalized = math::constrain(forward_speed_normalized + pid_calculate(&_pid_throttle,
+					   _rover_differential_setpoint.forward_speed_setpoint,
+					   vehicle_forward_speed, 0,
+					   dt), -1.f, 1.f); // Feedback
+
 	} else { // Use normalized setpoint
-		throttle = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized) ?
-			   math::constrain(_rover_differential_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
+		forward_speed_normalized = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized) ?
+					   math::constrain(_rover_differential_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
 	}

 	// Publish rover differential status (logging)
 	rover_differential_status_s rover_differential_status{};
 	rover_differential_status.timestamp = _timestamp;
 	rover_differential_status.actual_speed = vehicle_forward_speed;
-	rover_differential_status.actual_yaw_deg = M_RAD_TO_DEG_F * vehicle_yaw;
-	rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _rover_differential_setpoint.yaw_rate_setpoint;
-	rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * vehicle_yaw_rate;
+	rover_differential_status.actual_yaw = vehicle_yaw;
+	rover_differential_status.desired_yaw_rate = _rover_differential_setpoint.yaw_rate_setpoint;
+	rover_differential_status.actual_yaw_rate = vehicle_yaw_rate;
+	rover_differential_status.forward_speed_normalized = forward_speed_normalized;
+	rover_differential_status.speed_diff_normalized = speed_diff_normalized;
 	rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
 	rover_differential_status.pid_throttle_integral = _pid_throttle.integral;
 	rover_differential_status.pid_yaw_integral = _pid_yaw.integral;
@@ -154,7 +145,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
 	// Publish to motors
 	actuator_motors_s actuator_motors{};
 	actuator_motors.reversible_flags = _param_r_rev.get();
-	computeInverseKinematics(throttle, speed_diff_normalized).copyTo(actuator_motors.control);
+	computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control);
 	actuator_motors.timestamp = _timestamp;
 	_actuator_motors_pub.publish(actuator_motors);

@@ -113,7 +113,8 @@ private:
 	// Parameters
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
-		(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
+		(ParamFloat<px4::params::RD_MAX_THR_SPD>) _param_rd_max_thr_spd,
+		(ParamFloat<px4::params::RD_MAX_THR_YAW_R>) _param_rd_max_thr_yaw_r,
 		(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
 		(ParamFloat<px4::params::RD_YAW_RATE_P>) _param_rd_yaw_rate_p,
 		(ParamFloat<px4::params::RD_YAW_RATE_I>) _param_rd_yaw_rate_i,
@@ -140,10 +140,10 @@ private:

 	// Parameters
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
 		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
 		(ParamFloat<px4::params::RD_MAX_JERK>) _param_rd_max_jerk,
 		(ParamFloat<px4::params::RD_MAX_ACCEL>) _param_rd_max_accel,
+		(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
 		(ParamFloat<px4::params::RD_MISS_SPD_DEF>) _param_rd_miss_spd_def,
 		(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
 		(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn
@@ -115,8 +115,25 @@ parameters:

      RD_MAX_SPEED:
        description:
-          short: Maximum speed the rover can drive
-          long: This parameter is used to map desired speeds to normalized motor commands.
+          short: Maximum speed setpoint
+          long: |
+            This parameter is used to cap desired forward speed and map controller inputs to desired speeds in Position mode.
+        type: float
+        unit: m/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RD_MAX_THR_SPD:
+        description:
+          short: Speed the rover drives at maximum throttle
+          long: |
+            This parameter is used to calculate the feedforward term of the closed loop speed control which linearly
+            maps desired speeds to normalized motor commands [-1. 1].
+            A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode.
+            Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower.
        type: float
        unit: m/s
        min: 0
@@ -129,7 +146,8 @@ parameters:
        description:
          short: Maximum allowed yaw rate for the rover
          long: |
-            This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode.
+            This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates
+            in Acro,Stabilized and Position mode.
        type: float
        unit: deg/s
        min: 0.01
@@ -138,6 +156,23 @@ parameters:
        decimal: 2
        default: 90

+      RD_MAX_THR_YAW_R:
+        description:
+          short: Yaw rate turning left/right wheels at max speed in opposite directions
+          long: |
+            This parameter is used to calculate the feedforward term of the closed loop yaw rate control.
+            The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate.
+            This desired speed difference is then linearly mapped to normalized motor commands.
+            A good starting point is twice the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)).
+            Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower.
+        type: float
+        unit: m/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 2
+
      RD_MISS_SPD_DEF:
        description:
          short: Default forward speed for the rover during auto modes
@@ -32,13 +32,8 @@
 ############################################################################

 # Find the gz_Transport library
-# First look for GZ Harmonic libraries
-find_package(gz-transport NAMES gz-transport13)
-
-# If Harmonic not found, look for GZ Garden libraries
-if(NOT gz-transport_FOUND)
-	find_package(gz-transport NAMES gz-transport12)
-endif()
+# Look for GZ Ionic, Harmonic, and Garden library options in that order
+find_package(gz-transport NAMES gz-transport14 gz-transport13 gz-transport12)

 if(gz-transport_FOUND)

@@ -76,7 +76,7 @@ void Sih::run()
 	_airspeed_time = task_start;
 	_dist_snsr_time = task_start;
 	_vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast<typeof _sih_vtype.get()>(0),
-					  static_cast<typeof _sih_vtype.get()>(2));
+					  static_cast<typeof _sih_vtype.get()>(3));

 	_actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};

@@ -216,7 +216,8 @@ void Sih::sensor_step()

 	reconstruct_sensors_signals(now);

-	if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && now - _airspeed_time >= 50_ms) {
+	if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL)
+	    && now - _airspeed_time >= 50_ms) {
 		_airspeed_time = now;
 		send_airspeed(now);
 	}
@@ -311,7 +312,7 @@ void Sih::generate_force_and_torques()
 		_T_B = Vector3f(_T_MAX * _u[3], 0.0f, 0.0f); 	// forward thruster
 		// _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); 	// thruster torque
 		_Mt_B = Vector3f();
-		generate_fw_aerodynamics();
+		generate_fw_aerodynamics(_u[0], _u[1], _u[2], _u[3]);

 	} else if (_vehicle == VehicleType::TS) {
 		_T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (_u[0] + _u[1]));
@@ -320,17 +321,24 @@ void Sih::generate_force_and_torques()

 		// _Fa_I = -_KDV * _v_I;   // first order drag to slow down the aircraft
 		// _Ma_B = -_KDW * _w_B;   // first order angular damper
+
+	} else if (_vehicle == VehicleType::SVTOL) {
+		_T_B = Vector3f(_T_MAX * 2 * _u[8], 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3]));
+		_Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]),
+				 _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]),
+				 _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3]));
+		generate_fw_aerodynamics(_u[4], _u[6], _u[7], 0);
 	}
 }

-void Sih::generate_fw_aerodynamics()
+void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust)
 {
 	_v_B = _C_IB.transpose() * _v_I; 	// velocity in body frame [m/s]
 	float altitude = _H0 - _p_I(2);
-	_wing_l.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX);
-	_wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX);
-	_tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]);
-	_fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]);
+	_wing_l.update_aero(_v_B, _w_B, altitude, roll_cmd * FLAP_MAX);
+	_wing_r.update_aero(_v_B, _w_B, altitude, -roll_cmd * FLAP_MAX);
+	_tailplane.update_aero(_v_B, _w_B, altitude, pitch_cmd * FLAP_MAX, _T_MAX * thrust);
+	_fin.update_aero(_v_B, _w_B, altitude, yaw_cmd * FLAP_MAX, _T_MAX * thrust);
 	_fuselage.update_aero(_v_B, _w_B, altitude);

 	// sum of aerodynamic forces
@@ -383,7 +391,7 @@ void Sih::equations_of_motion(const float dt)

 	// fake ground, avoid free fall
 	if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) {
-		if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) {
+		if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL) {
 			if (!_grounded) {    // if we just hit the floor
 				// for the accelerometer, compute the acceleration that will stop the vehicle in one time step
 				_v_I_dot = -_v_I / dt;
@@ -450,7 +458,16 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us)
 	// TODO: send differential pressure instead?
 	airspeed_s airspeed{};
 	airspeed.timestamp_sample = time_now_us;
-	airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B(0) + generate_wgn() * 0.2f);
+
+	// airspeed sensor is mounted along the negative Z axis since the vehicle is a tailsitter
+	if (_vehicle == VehicleType::TS) {
+
+		airspeed.true_airspeed_m_s = fmaxf(0.1f, -_v_B(2) + generate_wgn() * 0.2f);
+
+	} else {
+		airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B(0) + generate_wgn() * 0.2f);
+	}
+
 	airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO);
 	airspeed.air_temperature_celsius = NAN;
 	airspeed.confidence = 0.7f;
@@ -618,6 +635,9 @@ int Sih::print_status()
 		PX4_INFO("aoa [deg]: %d", (int)(degrees(_ts[4].get_aoa())));
 		PX4_INFO("v segment (m/s)");
 		_ts[4].get_vS().print();
+
+	} else if (_vehicle == VehicleType::SVTOL) {
+		PX4_INFO("Running Standard VTOL");
 	}

 	PX4_INFO("vehicle landed: %d", _grounded);
@@ -132,7 +132,7 @@ private:
 	uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)};

 	// hard constants
-	static constexpr uint16_t NB_MOTORS = 6;
+	static constexpr uint16_t NB_MOTORS = 9;
 	static constexpr float T1_C = 15.0f;                        // ground temperature in Celsius
 	static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius;   // ground temperature in Kelvin
 	static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f;    // temperature gradient in degrees per metre
@@ -159,7 +159,7 @@ private:
 	void send_airspeed(const hrt_abstime &time_now_us);
 	void send_dist_snsr(const hrt_abstime &time_now_us);
 	void publish_ground_truth(const hrt_abstime &time_now_us);
-	void generate_fw_aerodynamics();
+	void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust);
 	void generate_ts_aerodynamics();
 	void sensor_step();

@@ -200,7 +200,7 @@ private:
 	matrix::Vector3f    _w_B_dot{};       // body rates differential
 	float       _u[NB_MOTORS] {};         // thruster signals

-	enum class VehicleType {MC, FW, TS};
+	enum class VehicleType {MC, FW, TS, SVTOL};
 	VehicleType _vehicle = VehicleType::MC;

 	// aerodynamic segments for the fixedwing
@@ -332,6 +332,7 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f);
 * @value 0 Multicopter
 * @value 1 Fixed-Wing
 * @value 2 Tailsitter
+ * @value 3 Standard VTOL
 * @reboot_required true
 * @group Simulation In Hardware
 */
Author	SHA1	Message	Date
RomanBapst	4d930bdec2	added full support for standard vtol in SIH Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-11-12 11:16:44 +03:00
RomanBapst	f417ae73ef	added full support for external autostart scripts Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-11-12 11:16:22 +03:00
RomanBapst	31433b6402	enable sih for v6x Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-11-12 11:16:22 +03:00
Claudio Chies	5d2e7c8748	Misc: Matrix: Added Addition and Subtraction to Slices (#23679 ) * Added Addition and Subtraction to Slices * MatrixSliceTest: refactor Addition/Substraction checks * Slice: replace operations returning a Matrix with calling the existing Matrix function --------- Co-authored-by: Matthias Grob <maetugr@gmail.com>	2024-09-18 11:17:37 +02:00
Marco Hauswirth	4a99a51fb1	update upload-artifact v2->v4	2024-09-17 10:55:00 -07:00
chfriedrich98	8aece9bff2	differential: fix CI issue	2024-09-17 09:34:51 -07:00
chfriedrich98	2fd4150b38	differential: Add stabilized and position mode (#23669 ) * differential: add position and stabilized mode * differential: add hardcoded stick input deadzones	2024-09-16 12:09:51 +02:00
chfriedrich98	81747f35bb	rover: add descend navigation state to land detection	2024-09-16 09:36:38 +02:00
Silvan Fuhrer	1c9c5e51c2	boards: cuav x7pro: remove build of ROVER and Q_ATTITUDE_ESTIMATOR to save flash Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-09-16 09:11:23 +02:00
Konrad	82a7d0410c	DistanceSensorModeChangeRequest: renaming of variable	2024-09-16 09:11:23 +02:00
Konrad	aab2390e51	navigator: publish distance sensor mode change request when in RTL landing phase or during mission landing	2024-09-16 09:11:23 +02:00
Konrad	1755b8304e	RTL direct: Make sure the _rtl_state captures the current status and not the next one	2024-09-16 09:11:23 +02:00
Konrad	e6f07bde2a	lightware_laser: add option to listen to system to enable/disable distance sensor	2024-09-16 09:11:23 +02:00
Silvan Fuhrer	9ca0630376	airframes: SIH_tailsitter: add SENS_DPRES_OFF to bypass airspeed cal Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-09-13 15:53:52 +03:00
RomanBapst	11440cfb45	added some default parameteter that allow the transition to complete Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-09-13 15:53:52 +03:00
RomanBapst	878c8bfcce	SIH: fix airspeed for tailsitter Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-09-13 15:53:52 +03:00
Konrad	4713a6737e	TECS: ramp up fast descend over 2_s to ramp down the throttle command	2024-09-13 14:04:39 +02:00
Konrad	585c92796d	mission_base: do not make terrain avoidance check when mission is not run anymore	2024-09-13 13:59:24 +02:00
Claudio Chies	4ba4b340cc	Reduce the orbit jerk by using a slew rate	2024-09-13 10:28:42 +02:00
Marco Hauswirth	22c2878cf8	send acknowledgement after receiving vehicle wind cmd	2024-09-12 09:34:08 +02:00
chfriedrich98	741ea6b707	differential: add individual parameters for speed and yaw rate feedforward	2024-09-11 13:57:27 +02:00
chfriedrich98	5d8a107925	differential: fix closed loop control removed thresholds for closed loop setpoints and added minimum thresholds for yaw rate and speed measurements instead to avoid moving due to measurement noise	2024-09-11 13:57:27 +02:00
Roman Bapst	c94c1ce4d2	Navigator: Support straight line mission landings (#23576 ) * introduced altitude acceptance radius in position setpoint for fixed wing guidance - allows navigator to explicitly set the altitude acceptance radius - needed for staright line landing support * added ignore_alt_acceptance to position setpoint message to allow guidance logic to ignore altitude error on waypoint - can be useful to prevent loitering at a waypoint within a mission landing sequence * only set altitude acceptance radius to infinity for a waypoint inside a mission landing for fixed wing vehicles * navigator: return altitude acceptance radius from triplet if it's valid * FixedWingPositionControl: check if alt acceptance radius provided in position setpoint is larger 0 --------- Signed-off-by: RomanBapst <bapstroman@gmail.com> Co-authored-by: Alvaro Fernandez <alvaro@auterion.com>	2024-09-10 17:44:24 +02:00
Mathieu Bresciani	03aec2e188	HeadingSmoothing: fix angle wrapping and add unit tests Co-authored-by: Matthias Grob <maetugr@gmail.com>	2024-09-10 15:06:19 +02:00
jmackay2	a5729da4e9	Simplify gz bridge CMakeLists and add GZ Ionic (#23657 ) Co-authored-by: jmackay2 <jmackay2@gmail.com>	2024-09-10 11:53:00 +02:00