fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 18:24:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'rwanda_sih' into rwanda_base

Browse Source
This commit is contained in:
RomanBapst 2024-10-04 11:08:14 +03:00
commit 04ea8d26f2
5 changed files with 133 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil Normal file
View File

@ -0,0 +1,99 @@
#!/bin/sh
#
# @name SIH Tailsitter Duo
#
# @type Simulation
# @class VTOL
#
# @output Motor1 motor right
# @output Motor2 motor left
# @output Servo1 elevon right
# @output Servo2 elevon left
#
# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
#
# @board px4_fmu-v2 exclude
#
. ${R}etc/init.d/rc.vtol_defaults
param set UAVCAN_ENABLE 0
param set-default VT_B_TRANS_DUR 5
param set-default VT_ELEV_MC_LOCK 0
param set-default VT_TYPE 2
param set-default MPC_MAN_Y_MAX 60
param set-default MC_PITCH_P 5
param set-default CA_AIRFRAME 2
param set-default CA_ROTOR_COUNT 5
param set-default CA_ROTOR0_KM 0.05
param set-default CA_ROTOR0_PX 0.2
param set-default CA_ROTOR0_PY 0.2
param set-default CA_ROTOR1_KM 0.05
param set-default CA_ROTOR1_PX -0.2
param set-default CA_ROTOR1_PY -0.2
param set-default CA_ROTOR2_PX 0.2
param set-default CA_ROTOR2_PY -0.2
param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.2
param set-default CA_ROTOR3_PY 0.2
param set-default CA_ROTOR3_KM -0.05
param set-default CA_ROTOR4_PX -0.3
param set-default CA_ROTOR4_KM 0.05
param set-default CA_ROTOR4_AX 1
param set-default CA_ROTOR4_AZ 0
param set-default CA_SV_CS_COUNT 3
param set-default CA_SV_CS0_TRQ_R 0.5
param set-default CA_SV_CS0_TYPE 2
param set-default CA_SV_CS1_TRQ_P 1
param set-default CA_SV_CS1_TYPE 3
param set-default CA_SV_CS2_TRQ_Y 1
param set HIL_ACT_REV 32
param set-default FW_AIRSPD_MAX 12
param set-default FW_AIRSPD_MIN 7
param set-default FW_AIRSPD_TRIM 10
param set-default HIL_ACT_FUNC1 101
param set-default HIL_ACT_FUNC2 102
param set-default HIL_ACT_FUNC3 103
param set-default HIL_ACT_FUNC4 104
param set-default HIL_ACT_FUNC5 201
param set-default HIL_ACT_FUNC6 202
param set-default HIL_ACT_FUNC7 203
param set-default HIL_ACT_FUNC8 105
param set-default MAV_TYPE 22
# set SYS_HITL to 2 to start the SIH and avoid sensors startup
param set-default SYS_HITL 2
# disable some checks to allow to fly:
# - without real battery
param set-default CBRK_SUPPLY_CHK 894281
# - without safety switch
param set-default CBRK_IO_SAFETY 22027
param set-default SENS_DPRES_OFF 0.001
param set SIH_T_MAX 2.0
param set SIH_Q_MAX 0.0165
param set SIH_MASS 0.2
# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
param set SIH_IXX 0.00354
param set SIH_IYY 0.000625
param set SIH_IZZ 0.00300
param set SIH_IXZ 0
param set SIH_KDV 0.2
param set SIH_L_ROLL 0.145
# sih as tailsitter
param set SIH_VEHICLE_TYPE 3
param set-default VT_ARSP_TRANS 6

1
ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
View File

@ -48,6 +48,7 @@ if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
1100_rc_quad_x_sih.hil
1101_rc_plane_sih.hil
1102_tailsitter_duo_sih.hil
1103_standard_vtol_sih.hil
)
endif()

39
src/modules/simulation/simulator_sih/sih.cpp
View File

@ -76,7 +76,7 @@ void Sih::run()
_airspeed_time = task_start;
_dist_snsr_time = task_start;
_vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast<typeof _sih_vtype.get()>(0),
static_cast<typeof _sih_vtype.get()>(2));
static_cast<typeof _sih_vtype.get()>(3));
_actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
@ -216,7 +216,8 @@ void Sih::sensor_step()
reconstruct_sensors_signals(now);
if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && now - _airspeed_time >= 50_ms) {
if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL)
&& now - _airspeed_time >= 50_ms) {
_airspeed_time = now;
send_airspeed(now);
}
@ -311,7 +312,7 @@ void Sih::generate_force_and_torques()
_T_B = Vector3f(_T_MAX * _u[3], 0.0f, 0.0f); // forward thruster
// _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); // thruster torque
_Mt_B = Vector3f();
generate_fw_aerodynamics();
generate_fw_aerodynamics(_u[0], _u[1], _u[2], _u[3]);
} else if (_vehicle == VehicleType::TS) {
_T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (_u[0] + _u[1]));
@ -320,17 +321,24 @@ void Sih::generate_force_and_torques()
// _Fa_I = -_KDV * _v_I; // first order drag to slow down the aircraft
// _Ma_B = -_KDW * _w_B; // first order angular damper
} else if (_vehicle == VehicleType::SVTOL) {
_T_B = Vector3f(_T_MAX * 2 * _u[8], 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3]));
_Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]),
_L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]),
_Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3]));
generate_fw_aerodynamics(_u[4], _u[6], _u[7], 0);
}
}
void Sih::generate_fw_aerodynamics()
void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust)
{
_v_B = _C_IB.transpose() * _v_I; // velocity in body frame [m/s]
float altitude = _H0 - _p_I(2);
_wing_l.update_aero(_v_B, _w_B, altitude, _u[0]*FLAP_MAX);
_wing_r.update_aero(_v_B, _w_B, altitude, -_u[0]*FLAP_MAX);
_tailplane.update_aero(_v_B, _w_B, altitude, _u[1]*FLAP_MAX, _T_MAX * _u[3]);
_fin.update_aero(_v_B, _w_B, altitude, _u[2]*FLAP_MAX, _T_MAX * _u[3]);
_wing_l.update_aero(_v_B, _w_B, altitude, roll_cmd * FLAP_MAX);
_wing_r.update_aero(_v_B, _w_B, altitude, -roll_cmd * FLAP_MAX);
_tailplane.update_aero(_v_B, _w_B, altitude, pitch_cmd * FLAP_MAX, _T_MAX * thrust);
_fin.update_aero(_v_B, _w_B, altitude, yaw_cmd * FLAP_MAX, _T_MAX * thrust);
_fuselage.update_aero(_v_B, _w_B, altitude);
// sum of aerodynamic forces
@ -383,7 +391,7 @@ void Sih::equations_of_motion(const float dt)
// fake ground, avoid free fall
if (_p_I(2) > 0.0f && (_v_I_dot(2) > 0.0f || _v_I(2) > 0.0f)) {
if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) {
if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL) {
if (!_grounded) { // if we just hit the floor
// for the accelerometer, compute the acceleration that will stop the vehicle in one time step
_v_I_dot = -_v_I / dt;
@ -450,8 +458,16 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us)
// TODO: send differential pressure instead?
airspeed_s airspeed{};
airspeed.timestamp_sample = time_now_us;
// airspeed sensor is mounted along the negative Z axis since the vehicle is a tailsitter
airspeed.true_airspeed_m_s = fmaxf(0.1f, -_v_B(2) + generate_wgn() * 0.2f);
if (_vehicle == VehicleType::TS) {
airspeed.true_airspeed_m_s = fmaxf(0.1f, -_v_B(2) + generate_wgn() * 0.2f);
} else {
airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_B(0) + generate_wgn() * 0.2f);
}
airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO);
airspeed.air_temperature_celsius = NAN;
airspeed.confidence = 0.7f;
@ -619,6 +635,9 @@ int Sih::print_status()
PX4_INFO("aoa [deg]: %d", (int)(degrees(_ts[4].get_aoa())));
PX4_INFO("v segment (m/s)");
_ts[4].get_vS().print();
} else if (_vehicle == VehicleType::SVTOL) {
PX4_INFO("Running Standard VTOL");
}
PX4_INFO("vehicle landed: %d", _grounded);

6
src/modules/simulation/simulator_sih/sih.hpp
View File

@ -132,7 +132,7 @@ private:
uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)};
// hard constants
static constexpr uint16_t NB_MOTORS = 6;
static constexpr uint16_t NB_MOTORS = 9;
static constexpr float T1_C = 15.0f; // ground temperature in Celsius
static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius; // ground temperature in Kelvin
static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre
@ -159,7 +159,7 @@ private:
void send_airspeed(const hrt_abstime &time_now_us);
void send_dist_snsr(const hrt_abstime &time_now_us);
void publish_ground_truth(const hrt_abstime &time_now_us);
void generate_fw_aerodynamics();
void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust);
void generate_ts_aerodynamics();
void sensor_step();
@ -200,7 +200,7 @@ private:
matrix::Vector3f _w_B_dot{}; // body rates differential
float _u[NB_MOTORS] {}; // thruster signals
enum class VehicleType {MC, FW, TS};
enum class VehicleType {MC, FW, TS, SVTOL};
VehicleType _vehicle = VehicleType::MC;
// aerodynamic segments for the fixedwing

1
src/modules/simulation/simulator_sih/sih_params.c
View File

@ -332,6 +332,7 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f);
* @value 0 Multicopter
* @value 1 Fixed-Wing
* @value 2 Tailsitter
* @value 3 Standard VTOL
* @reboot_required true
* @group Simulation In Hardware
*/