diff --git a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil new file mode 100644 index 0000000000..adf846807e --- /dev/null +++ b/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 +# +# @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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 634eb3e1b8..4a767f6595 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/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() diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 2ca4987338..4e044f52fe 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/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(0), - static_cast(2)); + static_cast(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); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 8988474e01..fcbd1431b1 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/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 diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index 16c58430f6..f9e053a5eb 100644 --- a/src/modules/simulation/simulator_sih/sih_params.c +++ b/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 */