mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
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:
parent
eb9a76cfaf
commit
3c5574c051
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
49
ROMFS/px4fmu_common/init.d/airframes/1105_rc_hexa_x_sih.hil
Normal file
49
ROMFS/px4fmu_common/init.d/airframes/1105_rc_hexa_x_sih.hil
Normal 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
|
||||
@ -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(
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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 <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,
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user