diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil index b5a633dc1bc3..4129d2569cf0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/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 # # @board px4_fmu-v2 exclude 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 000000000000..c67cf2cc85f0 --- /dev/null +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 06d123d8576b..d9142ec1c5ea 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 ea6425f21a31..f90805dd660e 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); } @@ -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); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 588d231855ae..490e1dd823df 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -134,7 +134,7 @@ class Sih : public ModuleBase, 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, 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, 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 diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index 16c58430f63e..f9e053a5ebea 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 */