feat(sih): add propeller model with advance ratio (#26720)

---------

Signed-off-by: romain-chiap <romain.chiap@gmail.com>
Signed-off-by: Ramon Roche <mrpollo@gmail.com>
Co-authored-by: romain-chiap <romain.chiap@gmail.com>
This commit is contained in:
Ramon Roche 2026-04-08 16:57:17 -06:00 committed by GitHub
parent eb9a76cfaf
commit 3c5574c051
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
9 changed files with 535 additions and 51 deletions

View File

@ -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

View File

@ -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

View File

@ -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

View File

@ -0,0 +1,49 @@
#!/bin/sh
#
# @name SIH Hexacopter X
#
# @type Simulation
# @class Copter
#
# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
#
# @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

View File

@ -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(

View File

@ -65,6 +65,243 @@
#include <conversion/rotation.h> // math::radians,
// #include <lib/mathlib/mathlib.h>
#include <math.h>
#include <px4_platform_common/defines.h> // PX4_ISFINITE
#include <px4_platform_common/log.h> // 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

View File

@ -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<float>(_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;

View File

@ -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 RungeKuttaMunthe-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 RungeKuttaMunthe-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 <px4_platform_common/module.h>
@ -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<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override,
(ParamFloat<px4::params::SIH_T_TAU>) _sih_thrust_tau,
// forward propeller
(ParamFloat<px4::params::SIH_F_T_MAX>) _sih_f_thrust_max,
(ParamFloat<px4::params::SIH_F_Q_MAX>) _sih_f_torque_max,
(ParamFloat<px4::params::SIH_F_CT0>) _sih_f_ct0,
(ParamFloat<px4::params::SIH_F_CT1>) _sih_f_ct1,
(ParamFloat<px4::params::SIH_F_CT2>) _sih_f_ct2,
(ParamFloat<px4::params::SIH_F_CP0>) _sih_f_cp0,
(ParamFloat<px4::params::SIH_F_CP1>) _sih_f_cp1,
(ParamFloat<px4::params::SIH_F_CP2>) _sih_f_cp2,
(ParamFloat<px4::params::SIH_F_DIA_INCH>) _sih_forward_diameter_inch,
(ParamFloat<px4::params::SIH_F_RPM_MAX>) _sih_forward_rpm_max,
(ParamInt<px4::params::BAT1_SOURCE>) _bat1_source,
(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype,
(ParamFloat<px4::params::SIH_WIND_N>) _sih_wind_n,
(ParamFloat<px4::params::SIH_WIND_E>) _sih_wind_e,

View File

@ -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