Merge branch 'rwanda_sih' into rwanda_base

Signed-off-by: RomanBapst <bapstroman@gmail.com>

.github/workflows/mavros_mission_tests.yml

@@ -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

.github/workflows/mavros_offboard_tests.yml

@@ -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

ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil

@@ -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

ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt

@@ -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()
 

ROMFS/px4fmu_common/init.d/rcS

@@ -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

boards/px4/fmu-v6x/default.px4board

@@ -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

src/lib/matrix/matrix/Slice.hpp

@@ -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>

src/lib/matrix/matrix/SquareMatrix.hpp

@@ -317,6 +317,7 @@ public:
 	}
 };
 
+using SquareMatrix2f = SquareMatrix<float, 2>;
 using SquareMatrix3f = SquareMatrix<float, 3>;
 using SquareMatrix3d = SquareMatrix<double, 3>;
 

src/lib/matrix/matrix/Vector2.hpp

@@ -102,7 +102,6 @@ public:
 
 };
 
-
 using Vector2f = Vector2<float>;
 using Vector2d = Vector2<double>;
 

src/lib/matrix/test/MatrixSliceTest.cpp

@@ -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)
 {

src/lib/matrix/test/MatrixVector3Test.cpp

@@ -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));
 }

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -687,11 +687,6 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 
 	_skipping_takeoff_detection = false;
 
-	if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
-		_control_mode_current = FW_POSCTRL_MODE_AUTO_VELOCITY;
-		return;
-	}
-
 	if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid
 	    && _control_mode.flag_control_position_enabled) {
 		if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy)
@@ -897,6 +892,10 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
 		break;
 
+	case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
+		control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
+		break;
+
 	case position_setpoint_s::SETPOINT_TYPE_LOITER:
 #ifdef CONFIG_FIGURE_OF_EIGHT
 		if (current_sp.loiter_pattern == position_setpoint_s::LOITER_TYPE_FIGUREEIGHT) {
@@ -1004,6 +1003,10 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 {
 	uint8_t position_sp_type = pos_sp_curr.type;
 
+	if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
+		return position_setpoint_s::SETPOINT_TYPE_VELOCITY;
+	}
+
 	Vector2d curr_wp{0, 0};
 
 	/* current waypoint (the one currently heading for) */
@@ -2591,11 +2594,6 @@ FixedwingPositionControl::Run()
 				break;
 			}
 
-		case FW_POSCTRL_MODE_AUTO_VELOCITY: {
-				control_auto_velocity(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
-				break;
-			}
-
 		case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
 				control_auto_fixed_bank_alt_hold(control_interval);
 				break;

src/modules/fw_pos_control/FixedwingPositionControl.hpp

@@ -255,7 +255,6 @@ private:
 		FW_POSCTRL_MODE_AUTO_TAKEOFF,
 		FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
 		FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
-		FW_POSCTRL_MODE_AUTO_VELOCITY,
 		FW_POSCTRL_MODE_AUTO_PATH,
 		FW_POSCTRL_MODE_MANUAL_POSITION,
 		FW_POSCTRL_MODE_MANUAL_ALTITUDE,

src/modules/rover_differential/RoverDifferential.hpp

@@ -61,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
 {
@@ -120,16 +128,6 @@ private:
 	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
 
-	// Thresholds to avoid moving at rest due to measurement noise
-	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
-
-	// Stick input deadzone
-	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
-
 	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,

src/modules/simulation/simulator_sih/sih.cpp

@@ -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,8 +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 sensor is mounted along the negative Z axis since the vehicle is a tailsitter
-	airspeed.true_airspeed_m_s = fmaxf(0.1f, -_v_B(2) + generate_wgn() * 0.2f);
+	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;
@@ -619,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);

src/modules/simulation/simulator_sih/sih.hpp

@@ -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

src/modules/simulation/simulator_sih/sih_params.c

@@ -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
  */