added full support for standard vtol in SIH

Signed-off-by: RomanBapst <[email protected]>

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 <[email protected]>
+#
+# @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()
 

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 @@ 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 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 @@ 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();
 
@@ -200,7 +200,7 @@ class Sih : public ModuleBase<Sih>, public ModuleParams
 	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
  */