From 49a8c4694614fdad852d257c7f129e929bbb7650 Mon Sep 17 00:00:00 2001 From: Pedro-Roque Date: Sun, 7 Dec 2025 23:15:52 -0800 Subject: [PATCH] init: working towards dual-action ATMOS --- .../airframes/70001_gz_atmos_dual | 167 ++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + Tools/simulation/gz | 2 +- src/drivers/linux_pwm_out/module.yaml | 2 +- src/lib/mixer_module/mixer_module.cpp | 1 - src/lib/mixer_module/mixer_module_tests.cpp | 2 +- .../gz_bridge/GZMixingInterfaceESC.cpp | 1 + src/modules/simulation/gz_bridge/module.yaml | 2 +- 8 files changed, 173 insertions(+), 5 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/70001_gz_atmos_dual diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/70001_gz_atmos_dual b/ROMFS/px4fmu_common/init.d-posix/airframes/70001_gz_atmos_dual new file mode 100644 index 0000000000..9a63cd420e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/70001_gz_atmos_dual @@ -0,0 +1,167 @@ +#!/bin/sh +# +# @name KTH-ATMOS +# +# @type Free-Flyer +# @class Spacecraft +# +# @output Motor1 back left thruster, +x thrust +# @output Motor2 front left thruster, -x thrust +# @output Motor3 back right thruster, +x thrust +# @output Motor4 front right thruster, -x thrust +# @output Motor5 front left thruster, +y thrust +# @output Motor6 front right thruster, -y thrust +# @output Motor7 back left thruster, +y thrust +# @output Motor8 back right thruster, -y thrust +# +# @maintainer discower-io +# @url https://atmos.discower.io +# + +. ${R}etc/init.d/rc.sc_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=atmos_dual} + +param set-default SIM_GZ_EN 1 + +param set-default SENS_EN_MAGSIM 1 +param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs +param set-default FD_ESCS_EN 0 + +param set-default CA_AIRFRAME 14 +param set-default MAV_TYPE 45 + +param set-default CA_ROTOR_COUNT 8 +param set-default CA_R_REV 0 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 + +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_ROTOR0_PX -0.12 +param set-default CA_ROTOR0_PY -0.12 +param set-default CA_ROTOR0_PZ 0.0 +param set-default CA_ROTOR0_CT 1.4 +param set-default CA_ROTOR0_AX 1.0 +param set-default CA_ROTOR0_AY 0.0 +param set-default CA_ROTOR0_AZ 0.0 + +param set-default CA_ROTOR1_PX 0.12 +param set-default CA_ROTOR1_PY -0.12 +param set-default CA_ROTOR1_PZ 0.0 +param set-default CA_ROTOR1_CT 1.4 +param set-default CA_ROTOR1_AX -1.0 +param set-default CA_ROTOR1_AY 0.0 +param set-default CA_ROTOR1_AZ 0.0 + +param set-default CA_ROTOR2_PX -0.12 +param set-default CA_ROTOR2_PY 0.12 +param set-default CA_ROTOR2_PZ 0.0 +param set-default CA_ROTOR2_CT 1.4 +param set-default CA_ROTOR2_AX 1.0 +param set-default CA_ROTOR2_AY 0.0 +param set-default CA_ROTOR2_AZ 0.0 + +param set-default CA_ROTOR3_PX 0.12 +param set-default CA_ROTOR3_PY 0.12 +param set-default CA_ROTOR3_PZ 0.0 +param set-default CA_ROTOR3_CT 1.4 +param set-default CA_ROTOR3_AX -1.0 +param set-default CA_ROTOR3_AY 0.0 +param set-default CA_ROTOR3_AZ 0.0 + +param set-default CA_ROTOR4_PX 0.12 +param set-default CA_ROTOR4_PY -0.12 +param set-default CA_ROTOR4_PZ 0.0 +param set-default CA_ROTOR4_CT 1.4 +param set-default CA_ROTOR4_AX 0.0 +param set-default CA_ROTOR4_AY 1.0 +param set-default CA_ROTOR4_AZ 0.0 + +param set-default CA_ROTOR5_PX 0.12 +param set-default CA_ROTOR5_PY 0.12 +param set-default CA_ROTOR5_PZ 0.0 +param set-default CA_ROTOR5_CT 1.4 +param set-default CA_ROTOR5_AX 0.0 +param set-default CA_ROTOR5_AY -1.0 +param set-default CA_ROTOR5_AZ 0.0 + +param set-default CA_ROTOR6_PX -0.12 +param set-default CA_ROTOR6_PY -0.12 +param set-default CA_ROTOR6_PZ 0.0 +param set-default CA_ROTOR6_CT 1.4 +param set-default CA_ROTOR6_AX 0.0 +param set-default CA_ROTOR6_AY 1.0 +param set-default CA_ROTOR6_AZ 0.0 + +param set-default CA_ROTOR7_PX -0.12 +param set-default CA_ROTOR7_PY 0.12 +param set-default CA_ROTOR7_PZ 0.0 +param set-default CA_ROTOR7_CT 1.4 +param set-default CA_ROTOR7_AX 0.0 +param set-default CA_ROTOR7_AY -1.0 +param set-default CA_ROTOR7_AZ 0.0 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 +param set-default SIM_GZ_EC_FUNC5 105 +param set-default SIM_GZ_EC_FUNC6 106 +param set-default SIM_GZ_EC_FUNC7 107 +param set-default SIM_GZ_EC_FUNC8 108 +param set-default SIM_GZ_EC_FUNC9 301 +param set-default SIM_GZ_EC_FUNC10 302 +param set-default SIM_GZ_EC_FUNC11 303 +param set-default SIM_GZ_EC_FUNC12 304 + +param set-default SIM_GZ_EC_MIN1 0 +param set-default SIM_GZ_EC_MIN2 0 +param set-default SIM_GZ_EC_MIN3 0 +param set-default SIM_GZ_EC_MIN4 0 +param set-default SIM_GZ_EC_MIN5 0 +param set-default SIM_GZ_EC_MIN6 0 +param set-default SIM_GZ_EC_MIN7 0 +param set-default SIM_GZ_EC_MIN8 0 +param set-default SIM_GZ_EC_MIN9 1100 +param set-default SIM_GZ_EC_MIN10 1100 +param set-default SIM_GZ_EC_MIN11 1100 +param set-default SIM_GZ_EC_MIN12 1100 + +param set-default SIM_GZ_EC_MAX1 10000 +param set-default SIM_GZ_EC_MAX2 10000 +param set-default SIM_GZ_EC_MAX3 10000 +param set-default SIM_GZ_EC_MAX4 10000 +param set-default SIM_GZ_EC_MAX5 10000 +param set-default SIM_GZ_EC_MAX6 10000 +param set-default SIM_GZ_EC_MAX7 10000 +param set-default SIM_GZ_EC_MAX8 10000 +param set-default SIM_GZ_EC_MAX9 1900 +param set-default SIM_GZ_EC_MAX10 1900 +param set-default SIM_GZ_EC_MAX11 1900 +param set-default SIM_GZ_EC_MAX12 1900 + +# Controller Tunings +param set SC_YAWRATE_P 3.335 +param set SC_YAWRATE_I 0.87 +param set SC_YAWRATE_D 0.15 +param set SC_YR_INT_LIM 0.2 +param set SC_YAW_P 3.0 + +param set SPC_POS_P 0.20 +param set SPC_VEL_P 6.55 +param set SPC_VEL_I 0.0 +param set SPC_VEL_D 0.0 +param set SPC_VEL_MAX 12.0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 4cf43413f6..a2e2a4d290 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -118,6 +118,7 @@ px4_add_romfs_files( 60002_gz_uuv_bluerov2_heavy 70000_gz_atmos + 70001_gz_atmos_dual # [22000, 22999] Reserve for custom models ) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 947f75b25d..2086698c30 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 947f75b25d434f7a227b77307795cb9a71de91f1 +Subproject commit 2086698c305fe1b48a1b93f4f835b0089a612e90 diff --git a/src/drivers/linux_pwm_out/module.yaml b/src/drivers/linux_pwm_out/module.yaml index abc29c704c..5204e8852b 100644 --- a/src/drivers/linux_pwm_out/module.yaml +++ b/src/drivers/linux_pwm_out/module.yaml @@ -8,4 +8,4 @@ actuator_output: min: { min: 800, max: 1400, default: 1000 } max: { min: 1600, max: 2200, default: 2000 } failsafe: { min: 800, max: 2200 } - num_channels: 8 + num_channels: 16 diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index fe1b818f86..a8ad7adb53 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -358,7 +358,6 @@ bool MixingOutput::updateSubscriptions(bool allow_wq_switch) unlock(); _interface.mixerChanged(); - return true; } diff --git a/src/lib/mixer_module/mixer_module_tests.cpp b/src/lib/mixer_module/mixer_module_tests.cpp index 7f3a6feede..0faf5231b9 100644 --- a/src/lib/mixer_module/mixer_module_tests.cpp +++ b/src/lib/mixer_module/mixer_module_tests.cpp @@ -50,7 +50,7 @@ #define PARAM_PREFIX "HIL_ACT" #endif -static constexpr int MAX_NUM_OUTPUTS = 8; +static constexpr int MAX_NUM_OUTPUTS = 16; static constexpr int DISARMED_VALUE = 900; static constexpr int FAILSAFE_VALUE = 800; static constexpr int MIN_VALUE = 1000; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp index f556817ea9..75d9d97af2 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp @@ -85,6 +85,7 @@ bool GZMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsign } if (_actuators_pub.Valid()) { + PX4_INFO("Update complete"); return _actuators_pub.Publish(rotor_velocity_message); } } diff --git a/src/modules/simulation/gz_bridge/module.yaml b/src/modules/simulation/gz_bridge/module.yaml index e41aedb1e3..9ebc757830 100644 --- a/src/modules/simulation/gz_bridge/module.yaml +++ b/src/modules/simulation/gz_bridge/module.yaml @@ -18,7 +18,7 @@ actuator_output: min: { min: 0, max: 1000, default: 0 } max: { min: 0, max: 1000, default: 1000 } failsafe: { min: 0, max: 1000 } - num_channels: *max_num_servos + num_channels: 12 - param_prefix: SIM_GZ_SV group_label: 'Servos' channel_label: 'Servo'