SIH: add Hexacopter X

to enable easy simulation of a motor failure.
This commit is contained in:
Matthias Grob
2025-05-12 18:57:29 +02:00
parent 9e45f077b1
commit 0b3fc0a62d
8 changed files with 101 additions and 24 deletions
@@ -54,6 +54,7 @@ if(PX4_PLATFORM MATCHES "posix")
quadx
xvert
standard_vtol
hex
)
# find corresponding airframes
+30 -10
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2025 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
@@ -75,8 +75,9 @@ void Sih::run()
_last_run = task_start;
_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()>(3));
_vehicle = static_cast<VehicleType>(constrain(_sih_vtype.get(),
static_cast<int32_t>(VehicleType::First),
static_cast<int32_t>(VehicleType::Last)));
_actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
@@ -319,7 +320,7 @@ void Sih::read_motors(const float dt)
void Sih::generate_force_and_torques()
{
if (_vehicle == VehicleType::Multicopter) {
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]),
@@ -327,6 +328,21 @@ void Sih::generate_force_and_torques()
_Fa_E = -_KDV * _v_E; // first order drag to slow down the aircraft
_Ma_B = -_KDW * _w_B; // first order angular damper
} else if (_vehicle == VehicleType::Hexacopter) {
/* m5 m0 ┬
\ / √3/2
m4 -- + -- m1 ┴
/ \
m3 m2
├1/2┤
├ 1 ┤ */
_T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3] + _u[4] + _u[5]));
_Mt_B = Vector3f(_L_ROLL * _T_MAX * (-.5f * _u[0] - _u[1] - .5f * _u[2] + .5f * _u[3] + _u[4] + .5f * _u[5]),
_L_PITCH * _T_MAX * (M_SQRT3_F / 2.f) * (+_u[0] - _u[2] - _u[3] + _u[5]),
_Q_MAX * (+_u[0] - _u[1] + _u[2] - _u[3] + _u[4] - _u[5]));
_Fa_E = -_KDV * _v_E; // first order drag to slow down the aircraft
_Ma_B = -_KDW * _w_B; // first order angular damper
} else if (_vehicle == VehicleType::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
@@ -429,7 +445,8 @@ void Sih::equations_of_motion(const float dt)
Vector3f ground_force_E;
if ((_lla.altitude() - _lpos_ref_alt) < 0.f && force_down > 0.f) {
if (_vehicle == VehicleType::Multicopter
if (_vehicle == VehicleType::Quadcopter
|| _vehicle == VehicleType::Hexacopter
|| _vehicle == VehicleType::TailsitterVTOL
|| _vehicle == VehicleType::StandardVTOL) {
ground_force_E = -sum_of_forces_E;
@@ -715,20 +732,23 @@ int Sih::print_status()
PX4_INFO("Achieved speedup: %.2fX", (double)_achieved_speedup);
#endif
if (_vehicle == VehicleType::Multicopter) {
PX4_INFO("Running MultiCopter");
if (_vehicle == VehicleType::Quadcopter) {
PX4_INFO("Quadcopter");
} else if (_vehicle == VehicleType::Hexacopter) {
PX4_INFO("Hexacopter");
} else if (_vehicle == VehicleType::FixedWing) {
PX4_INFO("Running Fixed-Wing");
PX4_INFO("Fixed-Wing");
} else if (_vehicle == VehicleType::TailsitterVTOL) {
PX4_INFO("Running TailSitter");
PX4_INFO("TailSitter");
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("Running Standard VTOL");
PX4_INFO("Standard VTOL");
}
PX4_INFO("vehicle landed: %d", _grounded);
+4 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2025 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
@@ -218,11 +218,10 @@ private:
matrix::Vector3f _v_E_dot{};
matrix::Dcmf _R_N2E; // local navigation to ECEF frame rotation matrix
float _u[NUM_ACTUATORS_MAX] {}; // thruster signals
float _u[NUM_ACTUATORS_MAX] {}; // thruster signals
enum class VehicleType {Multicopter, FixedWing, TailsitterVTOL, StandardVTOL};
VehicleType _vehicle = VehicleType::Multicopter;
enum class VehicleType {Quadcopter, FixedWing, TailsitterVTOL, StandardVTOL, Hexacopter, First = Quadcopter, Last = Hexacopter}; // numbering dependent on parameter SIH_VEHICLE_TYPE
VehicleType _vehicle = VehicleType::Quadcopter;
// aerodynamic segments for the fixedwing
AeroSeg _wing_l = AeroSeg(SPAN / 2.0f, MAC, -4.0f, matrix::Vector3f(0.0f, -SPAN / 4.0f, 0.0f), 3.0f,
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2025 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
@@ -33,7 +33,7 @@
/**
* @file sih_params.c
* Parameters for quadcopter X simulator in hardware.
* Parameters for simulator in hardware.
*
* @author Romain Chiappinelli <romain.chiap@gmail.com>
* February 2019
@@ -329,10 +329,11 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f);
/**
* Vehicle type
*
* @value 0 Multicopter
* @value 0 Quadcopter
* @value 1 Fixed-Wing
* @value 2 Tailsitter
* @value 3 Standard VTOL
* @value 4 Hexacopter
* @reboot_required true
* @group Simulation In Hardware
*/