From 0b3fc0a62d2db9a94b5e4f493fa5d2aee7f83400 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 12 May 2025 18:57:29 +0200 Subject: [PATCH] SIH: add Hexacopter X to enable easy simulation of a motor failure. --- .../init.d-posix/airframes/10044_sihsim_hex | 47 +++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + docs/en/sim_sih/index.md | 18 +++++-- .../community_supported_simulators.md | 2 +- .../simulation/simulator_sih/CMakeLists.txt | 1 + src/modules/simulation/simulator_sih/sih.cpp | 40 ++++++++++++---- src/modules/simulation/simulator_sih/sih.hpp | 9 ++-- .../simulation/simulator_sih/sih_params.c | 7 +-- 8 files changed, 101 insertions(+), 24 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex b/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex new file mode 100644 index 0000000000..36ae9a26d7 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10044_sihsim_hex @@ -0,0 +1,47 @@ +#!/bin/sh +# +# @name HexarotorX SITL for SIH +# +# @type Hexarotor x +# @class Copter +# +# @maintainer Matthias Grob +# + +. ${R}etc/init.d/rc.mc_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=hex} + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 + +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 PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 +param set-default PWM_MAIN_FUNC5 105 +param set-default PWM_MAIN_FUNC6 106 + +param set-default EKF2_GPS_DELAY 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index b4e107799f..ff72e5e468 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -109,6 +109,7 @@ px4_add_romfs_files( 10041_sihsim_airplane 10042_sihsim_xvert 10043_sihsim_standard_vtol + 10044_sihsim_hex 17001_flightgear_tf-g1 17002_flightgear_tf-g2 diff --git a/docs/en/sim_sih/index.md b/docs/en/sim_sih/index.md index 9bbd356d98..9f27250f1e 100644 --- a/docs/en/sim_sih/index.md +++ b/docs/en/sim_sih/index.md @@ -58,6 +58,7 @@ To set up/start SIH: 1. Open QGroundControl and wait for the flight controller too boot and connect. 1. Open [Vehicle Setup > Airframe](../config/airframe.md) then select the desired frame: - [SIH Quadcopter X](../airframes/airframe_reference.md#copter_simulation_sih_quadcopter_x) + - SIH Hexacopter X currently only has an airframe for SITL to safe flash so on flight control hardware it has to be manually configured equivalently. - [SIH plane AERT](../airframes/airframe_reference.md#plane_simulation_sih_plane_aert) - [SIH Tailsitter Duo](../airframes/airframe_reference.md#vtol_simulation_sih_tailsitter_duo) - [SIH Standard VTOL QuadPlane](../airframes/airframe_reference.md#vtol_simulation_sih_standard_vtol_quadplane) @@ -114,25 +115,31 @@ To run SIH as SITL: 1. Install the [PX4 Development toolchain](../dev_setup/dev_env.md). 1. Run the appropriate make command for each vehicle type (at the root of the PX4-Autopilot repository): - - quadrotor: + - Quadcopter ```sh make px4_sitl sihsim_quadx ``` - - Fixed-wing (plane): + - Hexacopter + + ```sh + make px4_sitl sihsim_hex + ``` + + - Fixed-wing (plane) ```sh make px4_sitl sihsim_airplane ``` - - XVert VTOL tailsitter: + - XVert VTOL tailsitter ```sh make px4_sitl sihsim_xvert ``` - - Standard VTOL: + - Standard VTOL ```sh make px4_sitl sihsim_standard_vtol @@ -231,7 +238,8 @@ For specific examples see the `_sihsim_` airframes in [ROMFS/px4fmu_common/init. The dynamic models for the various vehicles are: -- Quadrotor: [pdf report](https://github.com/PX4/PX4-user_guide/raw/main/assets/simulation/SIH_dynamic_model.pdf). +- Quadcopter: [pdf report](https://github.com/PX4/PX4-user_guide/raw/main/assets/simulation/SIH_dynamic_model.pdf). +- Hexacopter: Equivalent to the Quadcopter but with a symmetric hexacopter x actuation setup. - Fixed-wing: Inspired by the PhD thesis: "Dynamics modeling of agile fixed-wing unmanned aerial vehicles." Khan, Waqas, supervised by Nahon, Meyer, McGill University, PhD thesis, 2016. - Tailsitter: Inspired by the master's thesis: "Modeling and control of a flying wing tailsitter unmanned aerial vehicle." Chiappinelli, Romain, supervised by Nahon, Meyer, McGill University, Masters thesis, 2018. diff --git a/docs/en/simulation/community_supported_simulators.md b/docs/en/simulation/community_supported_simulators.md index 6aec7dba67..79f176ea72 100644 --- a/docs/en/simulation/community_supported_simulators.md +++ b/docs/en/simulation/community_supported_simulators.md @@ -14,8 +14,8 @@ Questions about these tools should be raised on the [discussion forums](../contr | Simulator | Description | | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| [Simulation-In-Hardware](../sim_sih/index.md) (SIH) |

A simulator implemented in C++ as a PX4 module directly in the Firmware [code](https://github.com/PX4/PX4-Autopilot/tree/main/src/modules/simulation/simulator_sih). It can be ran in SITL directly on the computer or as an alternative to HITL offering a hard real-time simulation directly on the hardware autopilot.

Supported Vehicles: Quad, Hexa, Plane, Tailsitter, Standard VTOL

| | [FlightGear](../sim_flightgear/index.md) |

A simulator that provides physically and visually realistic simulations. In particular it can simulate many weather conditions, including thunderstorms, snow, rain and hail, and can also simulate thermals and different types of atmospheric flows. [Multi-vehicle simulation](../sim_flightgear/multi_vehicle.md) is also supported.

Supported Vehicles: Plane, Autogyro, Rover

| | [JMAVSim](../sim_jmavsim/index.md) |

A simple multirotor/quad simulator. This was previously part of the PX4 development toolchain but was removed in favour of [Gazebo](../sim_gazebo_gz/index.md).

Supported Vehicles: Quad

| | [JSBSim](../sim_jsbsim/index.md) |

A simulator that provides advanced flight dynamics models. This can be used to model realistic flight dynamics based on wind tunnel data.

Supported Vehicles: Plane, Quad, Hex

| | [AirSim](../sim_airsim/index.md) |

A cross platform simulator that provides physically and visually realistic simulations. This simulator is resource intensive, and requires a very significantly more powerful computer than the other simulators described here.

Supported Vehicles: Iris (MultiRotor model and a configuration for PX4 QuadRotor in the X configuration).

| -| [Simulation-In-Hardware](../sim_sih/index.md) (SIH) |

An alternative to HITL that offers a hard real-time simulation directly on the hardware autopilot. This simulator is implemented in C++ as a PX4 module directly in the Firmware [code](https://github.com/PX4/PX4-Autopilot/tree/main/src/modules/simulation/simulator_sih).

Supported Vehicles: Plane, Quad, Tailsitter, Standard VTOL

| diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt index 7ae8483c51..0be8db3c1b 100644 --- a/src/modules/simulation/simulator_sih/CMakeLists.txt +++ b/src/modules/simulation/simulator_sih/CMakeLists.txt @@ -54,6 +54,7 @@ if(PX4_PLATFORM MATCHES "posix") quadx xvert standard_vtol + hex ) # find corresponding airframes diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 30ac027d6f..32b973c3f8 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -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(0), - static_cast(3)); + _vehicle = static_cast(constrain(_sih_vtype.get(), + static_cast(VehicleType::First), + static_cast(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); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 557ec2b16c..4bcc5a281a 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -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, diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index f9e053a5eb..c3112ca598 100644 --- a/src/modules/simulation/simulator_sih/sih_params.c +++ b/src/modules/simulation/simulator_sih/sih_params.c @@ -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 * 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 */