SIH: Add Standard VTOL Airframe (#24175)

* add standard vtol airframe to SIH.

mostly took changes from 4d930bd and applied to main.

generate_fw_aerodynamics now takes four arguments rather than using the
_u class member, because depending on vehicle type _u is used
differently.

ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil

@@ -5,6 +5,15 @@
 # @type Standard VTOL
 # @class VTOL
 #
+# @output Motor1 MC motor front right
+# @output Motor2 MC motor back left
+# @output Motor3 MC motor front left
+# @output Motor4 MC motor back right
+# @output Motor5 Forward thrust motor
+# @output Servo1 Aileron
+# @output Servo2 Elevator
+# @output Servo3 Rudder
+#
 # @maintainer Roman Bapst <[email protected]>
 #
 # @board px4_fmu-v2 exclude

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

@@ -0,0 +1,101 @@
+#!/bin/sh
+#
+# @name SIH Standard VTOL QuadPlane
+#
+# @type Simulation
+# @class VTOL
+#
+# @output Motor1 MC motor front right
+# @output Motor2 MC motor back left
+# @output Motor3 MC motor front left
+# @output Motor4 MC motor back right
+# @output Motor5 Forward thrust motor
+# @output Servo1 Aileron
+# @output Servo2 Elevator
+# @output Servo3 Rudder
+#
+# @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 standard vtol
+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()
 

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);
 	}
@@ -302,7 +303,7 @@ void Sih::read_motors(const float dt)
 	if (_actuator_out_sub.update(&actuators_out)) {
 		_last_actuator_output_time = actuators_out.timestamp;
 
-		for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
+		for (int i = 0; i < NUM_ACTUATORS_MAX; i++) { // saturate the motor signals
 			if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS && i > 3)) {
 				_u[i] = actuators_out.output[i];
 
@@ -328,7 +329,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]));
@@ -337,17 +338,31 @@ void Sih::generate_force_and_torques()
 
 		// _Fa_E = -_KDV * _v_E;   // 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[7], 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]));
+
+		// thrust 0 because it is already contained in _T_B. in
+		// equations_of_motion they are all summed into sum_of_forces_E
+		generate_fw_aerodynamics(_u[4], _u[5], _u[6], 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 throttle_cmd)
 {
 	const Vector3f v_B = _q_E.rotateVectorInverse(_v_E);
 	const float &alt = _lla.altitude();
-	_wing_l.update_aero(v_B, _w_B, alt, -_u[0]*FLAP_MAX);
-	_wing_r.update_aero(v_B, _w_B, alt, _u[0]*FLAP_MAX);
-	_tailplane.update_aero(v_B, _w_B, alt, -_u[1]*FLAP_MAX, _T_MAX * _u[3]);
-	_fin.update_aero(v_B, _w_B, alt, _u[2]*FLAP_MAX, _T_MAX * _u[3]);
+
+	_wing_l.update_aero(v_B, _w_B, alt, roll_cmd * FLAP_MAX);
+	_wing_r.update_aero(v_B, _w_B, alt, -roll_cmd * FLAP_MAX);
+
+	_tailplane.update_aero(v_B, _w_B, alt, pitch_cmd * FLAP_MAX, _T_MAX * throttle_cmd);
+	_fin.update_aero(v_B, _w_B, alt, yaw_cmd * FLAP_MAX, _T_MAX * throttle_cmd);
 	_fuselage.update_aero(v_B, _w_B, alt);
 
 	// sum of aerodynamic forces
@@ -412,7 +427,7 @@ void Sih::equations_of_motion(const float dt)
 	Vector3f ground_force_E;
 
 	if ((_lla.altitude() - _lpos_ref_alt) < 0.f && force_down > 0.f) {
-		if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) {
+		if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL) {
 			ground_force_E = -sum_of_forces_E;
 
 			if (!_grounded) {
@@ -537,6 +552,8 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us)
 	// TODO: send differential pressure instead?
 	airspeed_s airspeed{};
 	airspeed.timestamp_sample = time_now_us;
+
+	// regardless of vehicle type, body frame, etc this holds as long as wind=0
 	airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_E.norm() + 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;
@@ -554,7 +571,7 @@ void Sih::send_dist_snsr(const hrt_abstime &time_now_us)
 	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM;
 
 	distance_sensor_s distance_sensor{};
-	//distance_sensor.timestamp_sample = time_now_us;
+	// distance_sensor.timestamp_sample = time_now_us;
 	distance_sensor.device_id = device_id.devid;
 	distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
 	distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
@@ -706,6 +723,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

@@ -134,7 +134,7 @@ class Sih : public ModuleBase<Sih>, public ModuleParams
 	uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)};
 
 	// hard constants
-	static constexpr uint16_t NB_MOTORS = 6;
+	static constexpr uint16_t NUM_ACTUATORS_MAX = 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
@@ -161,7 +161,7 @@ class Sih : public ModuleBase<Sih>, public ModuleParams
 	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();
 	static float computeGravity(double lat);
@@ -218,9 +218,9 @@ class Sih : public ModuleBase<Sih>, public ModuleParams
 	matrix::Vector3f    _v_E_dot{};
 	matrix::Dcmf        _R_N2E;           // local navigation to ECEF frame rotation matrix
 
-	float       _u[NB_MOTORS] {};         // thruster signals
+	float       _u[NUM_ACTUATORS_MAX] {};         // 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
  */