diff --git a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil index 40e8110e04..4a802cef62 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -14,6 +14,7 @@ param set UAVCAN_ENABLE 0 +param set-default CA_AIRFRAME 1 param set-default CA_ROTOR_COUNT 1 param set-default CA_ROTOR0_PX 0.3 @@ -38,7 +39,6 @@ param set-default SYS_HITL 2 # - without real battery param set-default CBRK_SUPPLY_CHK 894281 -param set SIH_T_MAX 6 param set SIH_MASS 0.3 param set SIH_IXX 0.00402 param set SIH_IYY 0.0144 @@ -48,3 +48,21 @@ param set SIH_KDV 0.2 param set SIH_VEHICLE_TYPE 1 # sih as fixed wing param set RWTO_TKOFF 1 # enable takeoff from runway (as opposed to launched) + +# pusher propeller model with advance ratio, model from UIUC APC 8x6" +param set SIH_F_T_MAX 6 +param set SIH_F_Q_MAX 0.03 +# if SIH_F_CT0 > 0, SIH_F_T_MAX and SIH_F_Q_MAX will be overridden +param set SIH_F_CT0 0.131 +param set SIH_F_CT1 0.004 +param set SIH_F_CT2 -0.146 +param set SIH_F_CP0 0.0777 +param set SIH_F_CP1 0.0498 +param set SIH_F_CP2 -0.11 +param set SIH_F_DIA_INCH 8 +param set SIH_F_RPM_MAX 9000 + +param set-default FW_AIRSPD_MIN 7 +param set-default FW_AIRSPD_TRIM 10 +param set-default FW_AIRSPD_MAX 12 +param set-default FW_PSP_OFF 0.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil index e033c1a4b6..5a1a6c829f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil @@ -28,6 +28,7 @@ param set-default VT_FW_DIFTHR_EN 1 param set-default VT_FW_DIFTHR_S_Y 0.3 param set-default MPC_MAN_Y_MAX 60 param set-default MC_PITCH_P 5 +param set-default FW_PSP_OFF 5 param set-default CA_AIRFRAME 4 param set-default CA_ROTOR_COUNT 2 @@ -56,7 +57,6 @@ param set-default HIL_ACT_REV 32 param set-default MAV_TYPE 19 - # set SYS_HITL to 2 to start the SIH and avoid sensors startup param set-default SYS_HITL 2 @@ -66,8 +66,9 @@ param set-default CBRK_SUPPLY_CHK 894281 param set-default SENS_DPRES_OFF 0.001 -param set SIH_T_MAX 2.0 -param set SIH_Q_MAX 0.0165 +# tailsitter is equipped with two forward propellers +param set SIH_F_T_MAX 2 +param set SIH_F_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 @@ -77,6 +78,19 @@ param set SIH_IXZ 0 param set SIH_KDV 0.2 param set SIH_L_ROLL 0.145 +# propeller diameter, rpm, and coeffs coming from the thesis +# Modeling and control of a flying wing tailsitter unmanned aerial vehicle." +# Chiappinelli, Romain, supervised by Nahon, Meyer, McGill University, Masters thesis, 2018. +# if SIH_F_CT0 > 0, SIH_F_T_MAX and SIH_F_Q_MAX will be overridden +param set SIH_F_CT0 0.1342 +param set SIH_F_CT1 -0.1196 +param set SIH_F_CT2 -0.1281 +param set SIH_F_CP0 0.0522 +param set SIH_F_CP1 -0.0146 +param set SIH_F_CP2 -0.0602 +param set SIH_F_DIA_INCH 5 +param set SIH_F_RPM_MAX 14000 + # sih as tailsitter param set SIH_VEHICLE_TYPE 2 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 index 5ac9e6b1ab..26b8d092bb 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil @@ -56,6 +56,7 @@ param set-default CA_SV_CS2_TYPE 4 # rudder param set-default FW_AIRSPD_MIN 7 param set-default FW_AIRSPD_TRIM 10 param set-default FW_AIRSPD_MAX 12 +param set-default VT_FWD_THRUST_EN 1 param set-default HIL_ACT_FUNC1 101 param set-default HIL_ACT_FUNC2 102 @@ -77,6 +78,7 @@ param set-default CBRK_SUPPLY_CHK 894281 param set-default SENS_DPRES_OFF 0.001 +# quadrotor propellers param set SIH_T_MAX 2.0 param set SIH_Q_MAX 0.0165 param set SIH_MASS 0.2 @@ -88,5 +90,18 @@ param set SIH_IXZ 0 param set SIH_KDV 0.2 param set SIH_L_ROLL 0.2 +# pusher propeller model with advance ratio, model from UIUC APC 8x6" +param set SIH_F_T_MAX 6 +param set SIH_F_Q_MAX 0.03 +# if SIH_F_CT0 > 0, SIH_F_T_MAX and SIH_F_Q_MAX will be overridden +param set SIH_F_CT0 0.131 +param set SIH_F_CT1 0.004 +param set SIH_F_CT2 -0.146 +param set SIH_F_CP0 0.0777 +param set SIH_F_CP1 0.0498 +param set SIH_F_CP2 -0.11 +param set SIH_F_DIA_INCH 8 +param set SIH_F_RPM_MAX 9000 + # sih as standard vtol param set SIH_VEHICLE_TYPE 3 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1105_rc_hexa_x_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1105_rc_hexa_x_sih.hil new file mode 100644 index 0000000000..b5574c1db8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1105_rc_hexa_x_sih.hil @@ -0,0 +1,49 @@ +#!/bin/sh +# +# @name SIH Hexacopter X +# +# @type Simulation +# @class Copter +# +# @maintainer Romain Chiappinelli +# +# @board px4_fmu-v2 exclude +# + +. ${R}etc/init.d/rc.mc_defaults + +param set UAVCAN_ENABLE 0 + +# set SYS_HITL to 2 to start the SIH and avoid sensors startup +param set SYS_HITL 2 + +# disable some checks to allow to fly: +# - without real battery +param set-default CBRK_SUPPLY_CHK 894281 + +param set SIH_VEHICLE_TYPE 4 + +# Symmetric hexacopter X clockwise motor numbering +param set-default CA_ROTOR_COUNT 6 +param set-default CA_ROTOR0_PX 0.866 +param set-default CA_ROTOR0_PY 0.5 +param set-default CA_ROTOR1_PX 0 +param set-default CA_ROTOR1_PY 1 +param set-default CA_ROTOR1_KM -0.05 +param set-default CA_ROTOR2_PX -0.866 +param set-default CA_ROTOR2_PY 0.5 +param set-default CA_ROTOR3_PX -0.866 +param set-default CA_ROTOR3_PY -0.5 +param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_PX 0 +param set-default CA_ROTOR4_PY -1 +param set-default CA_ROTOR5_PX 0.866 +param set-default CA_ROTOR5_PY -0.5 +param set-default CA_ROTOR5_KM -0.05 + +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 105 +param set-default HIL_ACT_FUNC6 106 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 8f13aa55b1..fd6827e509 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -49,6 +49,7 @@ if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM) 1101_rc_plane_sih.hil 1102_tailsitter_duo_sih.hil 1103_standard_vtol_sih.hil + 1105_rc_hexa_x_sih.hil ) if(CONFIG_MODULES_ROVER_ACKERMANN) px4_add_romfs_files( diff --git a/src/modules/simulation/simulator_sih/aero.hpp b/src/modules/simulation/simulator_sih/aero.hpp index 498acd9f23..1b353be7ff 100644 --- a/src/modules/simulation/simulator_sih/aero.hpp +++ b/src/modules/simulation/simulator_sih/aero.hpp @@ -65,6 +65,243 @@ #include // math::radians, // #include #include +#include // PX4_ISFINITE +#include // PX4_INFO + +// class Thruster using the terminology from UIUC Propeller Data site https://m-selig.ae.illinois.edu/props/propDB.html +class Thruster +{ +private: + static constexpr float INCH_TO_M = 0.0254f; + float CT0, CT1, CT2, CP0, CP1, CP2; + float _d_m; + float _rpm_max; + float T_MAX; + float Q_MAX; +public: + + // public implicit constructor + Thruster() : Thruster(0.1f, 1000.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f) {} + + /** public explicit constructor for static model + * using the maximum thrust and torque. + * In this case, the prop diameter and RPM are not needed. + */ + Thruster(float Tmax, float Qmax) + { + _d_m = 0.0f; + _rpm_max = 0.0f; + CT0 = 0.0f; + CT1 = 0.0f; + CT2 = 0.0f; + CP0 = 0.0f; + CP1 = 0.0f; + CP2 = 0.0f; + T_MAX = Tmax; + Q_MAX = Qmax; + } + + /** public explicit constructor + * - prop_dia_inch: propeller diameter in inches + * - rpm_max: max RPM + * - ct0, ct1, ct2: second order thrust coefficient + * - cp0, cp1, cp2: second order power coefficient + */ + Thruster(float prop_dia_inch, float rpm_max, float ct0, float ct1, float ct2, float cp0, float cp1, float cp2) + { + _d_m = dia_inch_to_m(prop_dia_inch); + _rpm_max = rpm_max; + CT0 = ct0; + CT1 = ct1; + CT2 = ct2; + CP0 = cp0; + CP1 = cp1; + CP2 = cp2; + // Initialize T_MAX and Q_MAX before they may be used inside the compute_* helpers + T_MAX = 0.0f; + Q_MAX = 0.0f; + T_MAX = compute_thrust_from_throttle(1.0f); + Q_MAX = compute_torque_from_throttle(1.0f); + } + + /** advance_ratio(float n_rev_s, float vx_ms) + * compute the advance ratio J = vx_ms / n_rev_s / d_m + * with + * - n_rev_s is the propeller rotational speed in revolution/s + * - vx_ms is the valocity seen by the thruster in m/s, + * usually projected on the propeller revolution axis + */ + float advance_ratio(float n_rev_s, float vx_ms) + { + return vx_ms / n_rev_s / _d_m; + } + + /** fCT(float J, float ct0, float ct1, float ct2) + * Compute the thrust coefficient ct from the advance ratio J + * + */ + float fCT(float J) + { + if (!PX4_ISFINITE(J)) { + return 0; + } + + // do not allow ct < 0 (i.e. windmill) + return fmaxf(CT0 + CT1 * J + CT2 * J * J, 0.0f); + } + + /** fCP(float J, float cp0, float cp1, float cp2) + * Compute the power coefficient cp from the advance ratio J + * + */ + float fCP(float J) + { + if (!PX4_ISFINITE(J)) { + return 0; + } + + return fmaxf(CP0 + CP1 * J + CP2 * J * J, 0.0f); + } + + /** compute_thrust(float n_rev_s, float vx_ms=0.0f, float rho=1.225f) + * Compute the thrust force in (N) given + * - n_rev_s is the propeller rotational speed in revolution/s + * - vx_ms is the velocity seen by the thruster in m/s, + * usually projected on the propeller revolution axis + * - rho is the density in kg/m^3 + */ + float compute_thrust(float n_rev_s, float vx_ms = 0.0f, float rho = 1.225f) + { + if (CT0 <= 0.0f || n_rev_s <= 1.0e-4f) { + return 0.0f; + } + + float J = advance_ratio(n_rev_s, vx_ms); + return fCT(J) * rho * n_rev_s * n_rev_s * _d_m * _d_m * _d_m * _d_m; + } + + /** compute_thrust_from_throttle(float u, float vx_ms=0.0f, float rho=1.225f) + * Compute the thrust force in (N) given + * - u is the thruster unit throttle in range [0,1] + * - vx_ms is the velocity seen by the thruster in m/s, + * usually projected on the propeller revolution axis + * - rho is the density in kg/m^3 + */ + float compute_thrust_from_throttle(float u, float vx_ms = 0.0f, float rho = 1.225f) + { + if (CT0 <= 0.0f) { + return T_MAX * u; + } + + float n_rev_s = throttle_to_rev_s(u, _rpm_max); + return compute_thrust(n_rev_s, vx_ms, rho); + } + + /** compute_torque(float n_rev_s, float vx_ms=0.0f, float rho=1.225f) + * Compute the propeller torque in (Nm) given + * - n_rev_s is the propeller rotational speed in revolution/s + * - vx_ms is the velocity seen by the thruster in m/s, + * usually projected on the propeller revolution axis + * - rho is the density in kg/m^3 + */ + float compute_torque(float n_rev_s, float vx_ms = 0.0f, float rho = 1.225f) + { + if (CP0 <= 0.0f || n_rev_s <= 1.0e-4f) { + return 0.0f; + } + + float J = advance_ratio(n_rev_s, vx_ms); + float cq = fCP(J) / 2.0f / M_PI_F; + return cq * rho * n_rev_s * n_rev_s * _d_m * _d_m * _d_m * _d_m * _d_m; + } + + /** compute_torque_from_throttle(float u, float vx_ms=0.0f, float rho=1.225f) + * Compute the propeller torque in (Nm) given + * - u is the thruster unit throttle in range [0,1] + * - vx_ms is the velocity seen by the thruster in m/s, + * usually projected on the propeller revolution axis + * - rho is the density in kg/m^3 + */ + float compute_torque_from_throttle(float u, float vx_ms = 0.0f, float rho = 1.225f) + { + if (CP0 <= 0.0f) { + return Q_MAX * u; + } + + float n_rev_s = throttle_to_rev_s(u, _rpm_max); + return compute_torque(n_rev_s, vx_ms, rho); + } + + /** dia_inch_to_m(float dia_inch) + * compute the propeller diameter in meter, + * given dia_inch the propeller diameter in inches. + */ + static float dia_inch_to_m(float dia_inch) + { + return dia_inch * INCH_TO_M; + } + + /** rpm_to_rev_s(float rpm) + * compute the propeller revolutions per seconds, + * given the propeller rpm. + */ + static float rpm_to_rev_s(float rpm) + { + return rpm / 60.0f; + } + + /** rev_s_to_rpm(float n_rev_s) + * compute the propeller rpm, + * given the propeller revolutions per seconds. + */ + static float rev_s_to_rpm(float n_rev_s) + { + return n_rev_s * 60.0f; + } + + /** throttle_to_rpm(float throttle, const float rpm_max) + * compute the propeller rpm, + * given the unit throttle u (in range [0,1]) + * and the max RPM rpm_max. + */ + static float throttle_to_rpm(float u, const float rpm_max) + { + return rpm_max * sqrtf(fminf(fmaxf(u, 0.0f), 1.0f)); + } + + /** throttle_to_rev_s(float throttle, const float rpm_max) + * compute the propeller revolution per seconds, + * given the unit throttle u (in range [0,1]) + * and the max RPM rpm_max. + */ + static float throttle_to_rev_s(float u, const float rpm_max) + { + return rpm_to_rev_s(throttle_to_rpm(u, rpm_max)); + } + + float get_T_max() + { + return T_MAX; + } + + float get_Q_max() + { + return Q_MAX; + } + + void print_status() + { + if (CT0 <= 0.0f) { + PX4_INFO("Thruster simple model: Tmax %.4f N, Qmax %.4f Nm", (double)T_MAX, (double)Q_MAX); + + } else { + PX4_INFO("Thruster dyn. model: dia %.4f m, max rpm %.0f, Tmax %.4f N, Qmax %.4f Nm", + (double)_d_m, (double)_rpm_max, (double)T_MAX, (double)Q_MAX); + PX4_INFO(" Tmax: %.3f N at 10 m/s, %.3f N at 20 m/s", + (double)compute_thrust_from_throttle(1.0f, 10.0f), (double)compute_thrust_from_throttle(1.0f, 20.0f)); + } + } +}; // class Aerodynamic Segment ------------------------------------------------------------------------ class AeroSeg diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 466cb24ae9..843a22281f 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2025 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2026 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -252,6 +252,37 @@ void Sih::parameters_updated() _L_PITCH = _sih_l_pitch.get(); _KDV = _sih_kdv.get(); _KDW = _sih_kdw.get(); + _F_T_MAX = _sih_f_thrust_max.get(); + _F_Q_MAX = _sih_f_torque_max.get(); + + // update the thruster models + for (size_t i = 0; i < NUM_DYN_THRUSTER; i++) { + if (_sih_f_ct0.get() > 0.0f && _sih_f_cp0.get() > 0.0f) { + _thruster[i] = Thruster(_sih_forward_diameter_inch.get(), _sih_forward_rpm_max.get(), + _sih_f_ct0.get(), _sih_f_ct1.get(), _sih_f_ct2.get(), + _sih_f_cp0.get(), _sih_f_cp1.get(), _sih_f_cp2.get()); + + } else { + _thruster[i] = Thruster(_F_T_MAX, _F_Q_MAX); + } + } + + if (_sih_f_ct0.get() > 0.0f && _sih_f_cp0.get() > 0.0f) { + _F_T_MAX = _thruster[0].get_T_max(); + _F_Q_MAX = _thruster[0].get_Q_max(); + + if (fabsf(_F_T_MAX - _sih_f_thrust_max.get()) > 1.0e-5f) { + _sih_f_thrust_max.set(_F_T_MAX); + _sih_f_thrust_max.commit(); + PX4_INFO("SIH_F_CT0 > 0, using propeller dynamic model, overriding SIH_F_T_MAX"); + } + + if (fabsf(_F_Q_MAX - _sih_f_torque_max.get()) > 1.0e-5f) { + _sih_f_torque_max.set(_F_Q_MAX); + _sih_f_torque_max.commit(); + PX4_INFO("SIH_F_CP0 > 0, using propeller dynamic model, overriding SIH_F_Q_MAX"); + } + } if (!_lpos_ref.isInitialized() || (fabsf(static_cast(_lpos_ref.getProjectionReferenceLat()) - _sih_lat0.get()) > FLT_EPSILON) @@ -314,7 +345,11 @@ void Sih::read_motors(const float dt) void Sih::generate_force_and_torques(const float dt) { + // air-relative velocity in body frame [m/s] + _v_B = _q_E.rotateVectorInverse(_R_N2E * _v_apparent_N); + if (_vehicle == VehicleType::Quadcopter) { + _T_B = Vector3f(0.0f, 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]), @@ -340,28 +375,34 @@ void Sih::generate_force_and_torques(const float dt) _Ma_B = -_KDW * _w_B; // first order angular damper } else if (_vehicle == VehicleType::FixedWing) { - _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(_u[0], _u[1], _u[2], _u[3]); + + _T[0] = _thruster[0].compute_thrust_from_throttle(_u[3], _v_B(0)); + _Q[0] = _thruster[0].compute_torque_from_throttle(_u[3], _v_B(0)); + _T_B = Vector3f(_T[0], 0.0f, 0.0f); // forward thruster + _Mt_B = Vector3f(_Q[0], 0.0f, 0.0f); // thruster torque + generate_fw_aerodynamics(_u[0], _u[1], _u[2], _T[0]); } else if (_vehicle == VehicleType::TailsitterVTOL) { - _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (_u[0] + _u[1])); - _Mt_B = Vector3f(_L_ROLL * _T_MAX * (_u[1] - _u[0]), 0.0f, _Q_MAX * (_u[1] - _u[0])); - generate_ts_aerodynamics(); - // _Fa_E = -_KDV * _R_N2E * _v_apparent_N; // first order drag to slow down the aircraft - // _Ma_B = -_KDW * _w_B; // first order angular damper + for (size_t i = 0; i < NUM_DYN_THRUSTER; i++) { + _T[i] = _thruster[i].compute_thrust_from_throttle(_u[i], -_v_B(2)); + _Q[i] = _thruster[i].compute_torque_from_throttle(_u[i], -_v_B(2)); + } + + _T_B = Vector3f(0.0f, 0.0f, -_T[0] - _T[1]); + _Mt_B = Vector3f(_L_ROLL * (_T[1] - _T[0]), 0.0f, _Q[1] - _Q[0]); + generate_ts_aerodynamics(); } else if (_vehicle == VehicleType::StandardVTOL) { - _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]), + _T[0] = _thruster[0].compute_thrust_from_throttle(_u[7], _v_B(0)); + _Q[0] = _thruster[0].compute_torque_from_throttle(_u[7], _v_B(0)); + _T_B = Vector3f(_T[0], 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]) + _Q[0], _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 + // thrust 0 means no propwash on the tail generate_fw_aerodynamics(_u[4], _u[5], _u[6], 0); } else if (_vehicle == VehicleType::RoverAckermann) { @@ -370,21 +411,20 @@ void Sih::generate_force_and_torques(const float dt) } void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, - const float throttle_cmd) + const float thrust_for_prowash) { - const Vector3f v_B = _q.rotateVectorInverse(_v_apparent_N); const float &alt = _lla.altitude(); - _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); + _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); + _tailplane.update_aero(_v_B, _w_B, alt, -pitch_cmd * FLAP_MAX, thrust_for_prowash); + _fin.update_aero(_v_B, _w_B, alt, yaw_cmd * FLAP_MAX, thrust_for_prowash); + _fuselage.update_aero(_v_B, _w_B, alt); // sum of aerodynamic forces const Vector3f Fa_B = _wing_l.get_Fa() + _wing_r.get_Fa() + _tailplane.get_Fa() + _fin.get_Fa() + _fuselage.get_Fa() - - _KDV * v_B; + _KDV * _v_B; _Fa_E = _q_E.rotateVector(Fa_B); // aerodynamic moments @@ -393,11 +433,8 @@ void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, void Sih::generate_ts_aerodynamics() { - // velocity in body frame [m/s] - const Vector3f v_B = _q.rotateVectorInverse(_v_apparent_N); - // the aerodynamic is resolved in a frame like a standard aircraft (nose-right-belly) - Vector3f v_ts = _R_S2B.transpose() * v_B; + Vector3f v_ts = _R_S2B.transpose() * _v_B; Vector3f w_ts = _R_S2B.transpose() * _w_B; float altitude = _lpos_ref_alt - _lpos(2); @@ -406,17 +443,17 @@ void Sih::generate_ts_aerodynamics() for (int i = 0; i < NB_TS_SEG; i++) { if (i <= NB_TS_SEG / 2) { - _ts[i].update_aero(v_ts, w_ts, altitude, _u[5]*TS_DEF_MAX, _T_MAX * _u[1]); + _ts[i].update_aero(v_ts, w_ts, altitude, _u[5]*TS_DEF_MAX, _T[1]); } else { - _ts[i].update_aero(v_ts, w_ts, altitude, -_u[4]*TS_DEF_MAX, _T_MAX * _u[0]); + _ts[i].update_aero(v_ts, w_ts, altitude, -_u[4]*TS_DEF_MAX, _T[0]); } Fa_ts += _ts[i].get_Fa(); Ma_ts += _ts[i].get_Ma(); } - const Vector3f Fa_B = _R_S2B * Fa_ts - _KDV * v_B; // sum of aerodynamic forces + const Vector3f Fa_B = _R_S2B * Fa_ts - _KDV * _v_B; // sum of aerodynamic forces _Fa_E = _q_E.rotateVector(Fa_B); _Ma_B = _R_S2B * Ma_ts - _KDW * _w_B; // aerodynamic moments } @@ -842,15 +879,21 @@ int Sih::print_status() } else if (_vehicle == VehicleType::FixedWing) { PX4_INFO("Fixed-Wing"); + PX4_INFO("propeller model:"); + _thruster[0].print_status(); } else if (_vehicle == VehicleType::TailsitterVTOL) { PX4_INFO("TailSitter"); + PX4_INFO("propeller model:"); + _thruster[0].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::StandardVTOL) { PX4_INFO("Standard VTOL"); + PX4_INFO("pusher propeller model:"); + _thruster[0].print_status(); } else if (_vehicle == VehicleType::RoverAckermann) { PX4_INFO("Rover Ackermann"); @@ -872,6 +915,8 @@ int Sih::print_status() (_R_N2E.transpose() * _Fa_E).print(); PX4_INFO("Aerodynamic moments body frame (Nm)"); _Ma_B.print(); + PX4_INFO("Thruster forces in body frame (N)"); + _T_B.print(); PX4_INFO("Thruster moments in body frame (Nm)"); _Mt_B.print(); return 0; diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index a3acdcfa0d..7e64672ea8 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2019-2025 PX4 Development Team. All rights reserved. +* Copyright (c) 2019-2026 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,19 +40,19 @@ * Coriolis g Corporation - January 2019 */ -// The sensor signals reconstruction and noise levels are from [1] -// [1] Bulka E, and Nahon M, "Autonomous fixed-wing aerobatics: from theory to flight." -// In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. -// The aerodynamic model is from [2] -// [2] Khan W, supervised by Nahon M, "Dynamics modeling of agile fixed-wing unmanned aerial vehicles." -// McGill University (Canada), PhD thesis, 2016. -// The quaternion integration are from [3] -// [3] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration for the Quaternion Kinematics." -// Journal of Guidance, Control, and Dynamics. 2019 Dec;42(12):2747-54. -// The tailsitter model is from [4] -// [4] Chiappinelli R, supervised by Nahon M, "Modeling and control of a flying wing tailsitter unmanned aerial vehicle." -// McGill University (Canada), Masters Thesis, 2018. - +/** The sensor signals reconstruction and noise levels are from [1]. The aerodynamic model is from [2]. + * The quaternion integration are from [3]. The tailsitter model is from [4]. The propeller models are from [5] + * [1] Bulka E, and Nahon M, "Autonomous fixed-wing aerobatics: from theory to flight." + * In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018. + * [2] Khan W, supervised by Nahon M, "Dynamics modeling of agile fixed-wing unmanned aerial vehicles." + * McGill University (Canada), PhD thesis, 2016. + * [3] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration for the Quaternion Kinematics." + * Journal of Guidance, Control, and Dynamics. 2019 Dec;42(12):2747-54. + * [4] Chiappinelli R, supervised by Nahon M, "Modeling and control of a flying wing tailsitter unmanned aerial vehicle." + * McGill University (Canada), Masters Thesis, 2018. + * [5] J.B. Brandt, R.W. Deters, G.K. Ananda, O.D. Dantsker, and M.S. Selig 2026, UIUC Propeller Database, + * Vols 1-4, University of Illinois at Urbana-Champaign, Department of Aerospace Engineering, retrieved from https://m-selig.ae.illinois.edu/props/propDB.html. + */ #pragma once #include @@ -142,6 +142,7 @@ private: // hard constants static constexpr uint16_t NUM_ACTUATORS_MAX = 9; + static constexpr uint16_t NUM_DYN_THRUSTER = 2; // number of dynamic thruster model with advance ratio // Ranging beacon simulation constants static constexpr uint8_t NUM_RANGING_BEACONS = 4; @@ -190,7 +191,7 @@ private: void send_dist_snsr(const hrt_abstime &time_now_us); void send_ranging_beacon(const hrt_abstime &time_now_us); void publish_ground_truth(const hrt_abstime &time_now_us); - void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust); + void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust_for_prowash); void generate_ts_aerodynamics(); void generate_rover_ackermann_dynamics(const float throttle_cmd, const float steering_cmd, const float dt); void sensor_step(); @@ -234,6 +235,7 @@ private: matrix::Vector3f _Mt_B{}; // thruster moments [Nm] matrix::Vector3f _Ma_B{}; // aerodynamic moments [Nm] matrix::Vector3f _w_B{}; // body rates in body frame [rad/s] + matrix::Vector3f _v_B{}; // body frame velocity [m/s] // Quantities in local navigation frame (NED, body-fixed) matrix::Vector3f _v_N{}; // velocity [m/s] @@ -257,6 +259,9 @@ private: matrix::Vector3f _lpos{}; // position in a local tangent-plane frame [m] float _u[NUM_ACTUATORS_MAX] {}; // thruster signals + float _T[NUM_DYN_THRUSTER] {}; // thruster forces (N) + float _Q[NUM_DYN_THRUSTER] {}; // thruster torque (Nm) + Thruster _thruster[NUM_DYN_THRUSTER] {}; // thruster objects enum class VehicleType {Quadcopter, FixedWing, TailsitterVTOL, StandardVTOL, Hexacopter, RoverAckermann, First = Quadcopter, Last = RoverAckermann}; // numbering dependent on parameter SIH_VEHICLE_TYPE VehicleType _vehicle = VehicleType::Quadcopter; @@ -296,7 +301,7 @@ private: // parameters MapProjection _lpos_ref{}; float _lpos_ref_alt; - float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _T_TAU; + float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _T_TAU, _F_T_MAX, _F_Q_MAX; matrix::Matrix3f _I; // vehicle inertia matrix matrix::Matrix3f _Im1; // inverse of the inertia matrix @@ -326,6 +331,18 @@ private: (ParamFloat) _sih_distance_snsr_max, (ParamFloat) _sih_distance_snsr_override, (ParamFloat) _sih_thrust_tau, + // forward propeller + (ParamFloat) _sih_f_thrust_max, + (ParamFloat) _sih_f_torque_max, + (ParamFloat) _sih_f_ct0, + (ParamFloat) _sih_f_ct1, + (ParamFloat) _sih_f_ct2, + (ParamFloat) _sih_f_cp0, + (ParamFloat) _sih_f_cp1, + (ParamFloat) _sih_f_cp2, + (ParamFloat) _sih_forward_diameter_inch, + (ParamFloat) _sih_forward_rpm_max, + (ParamInt) _bat1_source, (ParamInt) _sih_vtype, (ParamFloat) _sih_wind_n, (ParamFloat) _sih_wind_e, diff --git a/src/modules/simulation/simulator_sih/sih_params.yaml b/src/modules/simulation/simulator_sih/sih_params.yaml index 6bd1db0d8b..c405274fb2 100644 --- a/src/modules/simulation/simulator_sih/sih_params.yaml +++ b/src/modules/simulation/simulator_sih/sih_params.yaml @@ -83,12 +83,14 @@ parameters: increment: 0.005 SIH_T_MAX: description: - short: Max propeller thrust force + short: Max multicopter propeller thrust force long: |- This is the maximum force delivered by one propeller when the motor is running at full speed. This value is usually about 5 times the mass of the quadrotor. + + Refer to SIH_F_T_MAX for the thrust for FW, Tailsitter, and VTOL pusher. type: float default: 5.0 unit: N @@ -97,12 +99,14 @@ parameters: increment: 0.5 SIH_Q_MAX: description: - short: Max propeller torque + short: Max multicopter propeller torque long: |- This is the maximum torque delivered by one propeller when the motor is running at full speed. This value is usually about few percent of the maximum thrust force. + + Refer to SIH_F_Q_MAX for the propeller torque for FW, Tailsitter, and VTOL pusher. type: float default: 0.1 unit: Nm @@ -281,3 +285,87 @@ parameters: min: 0.0 max: 100.0 decimal: 1 + SIH_F_T_MAX: + description: + short: Forward thruster max thrust (N) + long: |- + This is used for the Fixed-Wing, Tailsitter, or pusher of the Standard VTOL + if SIH_F_CT0 <= 0. + If SIH_F_CT0 > 0, propeller model with advance ratio J is used + and this parameter value is overridden at simulation startup. + type: float + default: 2.0 + unit: N + min: 0.0 + decimal: 3 + SIH_F_Q_MAX: + description: + short: Forward thruster max torque (Nm) + long: |- + This is used for the Fixed-Wing, Tailsitter, or pusher of the Standard VTOL + if SIH_F_CP0 <= 0. + If SIH_F_CP0 > 0, propeller model with advance ratio J is used + and this parameter value is overridden at simulation startup. + type: float + default: 0.0165 + unit: Nm + min: 0.0 + decimal: 3 + SIH_F_CT0: + description: + short: Forward thruster static thrust coefficient + type: float + default: 0.0 + min: 0.0 + decimal: 3 + SIH_F_CT1: + description: + short: Forward thruster thrust coefficient 1 + long: CT(J) = CT0 + CT1*J + CT2*J^2 + type: float + default: 0.0 + decimal: 3 + SIH_F_CT2: + description: + short: Forward thruster thrust coefficient 2 + long: CT(J) = CT0 + CT1*J + CT2*J^2 + type: float + default: 0.0 + max: 0.0 + decimal: 3 + SIH_F_CP0: + description: + short: Forward thruster static power coefficient + type: float + default: 0.0 + min: 0.0 + decimal: 3 + SIH_F_CP1: + description: + short: Forward thruster power coefficient 1 + long: CP(J) = CP0 + CP1*J + CP2*J^2 + type: float + default: 0.0 + decimal: 3 + SIH_F_CP2: + description: + short: Forward thruster power coefficient 2 + long: CP(J) = CP0 + CP1*J + CP2*J^2 + type: float + default: 0.0 + max: 0.0 + decimal: 3 + SIH_F_DIA_INCH: + description: + short: Forward thruster propeller diameter in inches + type: float + default: 0.1 + min: 0.1 + decimal: 1 + SIH_F_RPM_MAX: + description: + short: Forward thruster max RPM + type: float + default: 6000.0 + min: 0.1 + decimal: 1