From 7f913a6f6eaa4771f3462df0373e1f6977e39460 Mon Sep 17 00:00:00 2001 From: haitomatic Date: Mon, 2 Dec 2024 09:03:04 +0000 Subject: [PATCH] Simulation: deprecate pwm_out_sim module HITL mode and use only pwm_esc module --- ROMFS/px4fmu_common/init.d-posix/rcS | 3 +- .../init.d/airframes/1001_rc_quad_x.hil | 8 +- .../init.d/airframes/1002_standard_vtol.hil | 16 +- .../init.d/airframes/1100_rc_quad_x_sih.hil | 8 +- .../init.d/airframes/1101_rc_plane_sih.hil | 14 +- .../airframes/1102_tailsitter_duo_sih.hil | 10 +- .../airframes/1401_ssrc_holybro_x500.hil | 23 ++- .../airframes/1404_ssrc_standard_vtol.hil | 55 +++--- .../airframes/1440_ssrc_skywalker_x8.hil | 22 ++- .../init.d/airframes/4401_ssrc_fog_x_tmotor | 6 - ROMFS/px4fmu_common/init.d/rcS | 5 - boards/px4/sitl/default.px4board | 1 - boards/ssrc/saluki-nxp93 | 2 +- boards/ssrc/saluki-pi | 2 +- boards/ssrc/saluki-v1 | 2 +- boards/ssrc/saluki-v2 | 2 +- boards/ssrc/saluki-v3 | 2 +- src/drivers/pwm_esc/pwm_esc.cpp | 19 +- src/lib/mixer_module/mixer_module_tests.cpp | 4 - .../mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp | 12 +- .../simulation/pwm_out_sim/CMakeLists.txt | 8 +- src/modules/simulation/pwm_out_sim/PWMSim.cpp | 61 ++---- src/modules/simulation/pwm_out_sim/PWMSim.hpp | 6 +- .../{module_sim.yaml => module.yaml} | 0 .../simulation/pwm_out_sim/module_hil.yaml | 13 -- ssrc_config/config_hitl_eth_gzsim_fw.txt | 57 ++---- ssrc_config/config_hitl_eth_gzsim_mc.txt | 30 --- ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt | 178 ++++-------------- ssrc_config/config_hitl_eth_gzsim_vtol_sv.txt | 56 +++--- 29 files changed, 231 insertions(+), 394 deletions(-) rename src/modules/simulation/pwm_out_sim/{module_sim.yaml => module.yaml} (100%) delete mode 100644 src/modules/simulation/pwm_out_sim/module_hil.yaml diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 1665b4be354d..ee553e7cc67d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -265,9 +265,10 @@ manual_control start sensors start commander start +# only required for classic gazebo and jMAVSim simulator if [ "$VEHICLE_TYPE" != "rover" ] then - if ! pwm_out_sim start -m sim + if ! pwm_out_sim start then tune_control play error fi diff --git a/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil index 9cbf0286603b..2af06dfaeb0f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil @@ -28,10 +28,10 @@ param set-default CA_ROTOR3_PX -0.15 param set-default CA_ROTOR3_PY 0.15 param set-default CA_ROTOR3_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 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 # disable some checks to allow to fly # - without real battery diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil index 60a01fb403e6..2d60e580b140 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -77,14 +77,14 @@ param set-default CA_SV_CS1_TYPE 2 param set-default CA_SV_CS2_TRQ_P 1.0 param set-default CA_SV_CS2_TYPE 3 -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 201 -param set-default HIL_ACT_FUNC7 202 -param set-default HIL_ACT_FUNC8 203 +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 201 +param set-default PWM_MAIN_FUNC7 202 +param set-default PWM_MAIN_FUNC8 203 param set SYS_HITL 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil index b1b42bd3e35a..7ded23ce9d10 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil @@ -27,10 +27,10 @@ param set-default CA_ROTOR3_PX -0.15 param set-default CA_ROTOR3_PY 0.15 param set-default CA_ROTOR3_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 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 # set SYS_HITL to 2 to start the SIH and avoid sensors startup param set SYS_HITL 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil index 0ec506b68f95..3aea8d3e133a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -27,13 +27,13 @@ param set-default CA_SV_CS2_TRQ_Y 1.0 param set-default CA_SV_CS2_TYPE 4 param set-default CA_SV_CS3_TYPE 10 -param set-default HIL_ACT_REV 2 -param set-default HIL_ACT_FUNC1 201 -param set-default HIL_ACT_FUNC2 202 -param set-default HIL_ACT_FUNC3 203 -param set-default HIL_ACT_FUNC4 101 -param set-default HIL_ACT_FUNC5 204 -param set-default HIL_ACT_FUNC6 400 +param set-default PWM_MAIN_REV 2 +param set-default PWM_MAIN_FUNC1 201 +param set-default PWM_MAIN_FUNC2 202 +param set-default PWM_MAIN_FUNC3 203 +param set-default PWM_MAIN_FUNC4 101 +param set-default PWM_MAIN_FUNC5 204 +param set-default PWM_MAIN_FUNC6 400 # set SYS_HITL to 2 to start the SIH and avoid sensors startup param set-default SYS_HITL 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil index d47d11545be5..f662bbf29244 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil @@ -43,11 +43,11 @@ param set-default CA_SV_CS1_TRQ_P 0.3 param set-default CA_SV_CS1_TRQ_Y -0.3 param set-default CA_SV_CS1_TYPE 6 -param set-default HIL_ACT_FUNC1 101 -param set-default HIL_ACT_FUNC2 102 -param set-default HIL_ACT_FUNC5 202 -param set-default HIL_ACT_FUNC6 201 -param set-default HIL_ACT_REV 32 +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC5 202 +param set-default PWM_MAIN_FUNC6 201 +param set-default PWM_MAIN_REV 32 param set-default MAV_TYPE 19 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil b/ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil index 7daed3eff97c..81c761f5e009 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1401_ssrc_holybro_x500.hil @@ -32,10 +32,25 @@ param set-default CA_ROTOR3_PX -0.175 param set-default CA_ROTOR3_PY 0.175 param set-default CA_ROTOR3_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 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_MIN1 0 +param set-default PWM_MAIN_MIN2 0 +param set-default PWM_MAIN_MIN3 0 +param set-default PWM_MAIN_MIN4 0 + +param set-default PWM_MAIN_MAX1 1000 +param set-default PWM_MAIN_MAX2 1000 +param set-default PWM_MAIN_MAX3 1000 +param set-default PWM_MAIN_MAX4 1000 + +param set-default PWM_MAIN_DIS1 0 +param set-default PWM_MAIN_DIS2 0 +param set-default PWM_MAIN_DIS3 0 +param set-default PWM_MAIN_DIS4 0 # Set takeoff ramp to disabled for a more decisive takeoff action param set-default MPC_TKO_RAMP_T 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1404_ssrc_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1404_ssrc_standard_vtol.hil index 06725298bcef..e23fdd48c148 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1404_ssrc_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1404_ssrc_standard_vtol.hil @@ -30,28 +30,41 @@ param set-default CA_ROTOR4_AZ 0.0 param set-default CA_ROTOR4_PX 0.2 # HITL PWM functions -param set-default HIL_ACT_FUNC1 101 -param set-default HIL_ACT_MIN1 10 -param set-default HIL_ACT_MAX1 1500 -param set-default HIL_ACT_FUNC2 102 -param set-default HIL_ACT_MIN2 10 -param set-default HIL_ACT_MAX2 1500 -param set-default HIL_ACT_FUNC3 103 -param set-default HIL_ACT_MIN3 10 -param set-default HIL_ACT_MAX3 1500 -param set-default HIL_ACT_FUNC4 104 -param set-default HIL_ACT_MIN4 10 -param set-default HIL_ACT_MAX4 1500 -param set-default HIL_ACT_FUNC5 105 -param set-default HIL_ACT_MIN5 0 -param set-default HIL_ACT_MAX5 3500 -param set-default HIL_ACT_FUNC6 201 -param set-default HIL_ACT_DIS6 500 -param set-default HIL_ACT_FUNC7 202 -param set-default HIL_ACT_DIS7 500 -param set-default HIL_ACT_FUNC8 203 -param set-default HIL_ACT_DIS8 500 +param set PWM_MAIN_FUNC1 101 +param set PWM_MAIN_FUNC2 102 +param set PWM_MAIN_FUNC3 103 +param set PWM_MAIN_FUNC4 104 +param set PWM_MAIN_FUNC5 105 +param set PWM_MAIN_FUNC6 201 +param set PWM_MAIN_FUNC7 202 +param set PWM_MAIN_FUNC8 203 +param set PWM_MAIN_MIN1 10 +param set PWM_MAIN_MIN2 10 +param set PWM_MAIN_MIN3 10 +param set PWM_MAIN_MIN4 10 +param set PWM_MAIN_MIN5 0 +param set PWM_MAIN_MIN6 0 +param set PWM_MAIN_MIN7 0 +param set PWM_MAIN_MIN8 0 + +param set PWM_MAIN_MAX1 1500 +param set PWM_MAIN_MAX2 1500 +param set PWM_MAIN_MAX3 1500 +param set PWM_MAIN_MAX4 1500 +param set PWM_MAIN_MAX5 3500 +param set PWM_MAIN_MAX6 1000 +param set PWM_MAIN_MAX7 1000 +param set PWM_MAIN_MAX8 1000 + +param set PWM_MAIN_DIS1 0 +param set PWM_MAIN_DIS2 0 +param set PWM_MAIN_DIS3 0 +param set PWM_MAIN_DIS4 0 +param set PWM_MAIN_DIS5 0 +param set PWM_MAIN_DIS6 500 +param set PWM_MAIN_DIS7 500 +param set PWM_MAIN_DIS8 500 param set-default ASPD_PRIMARY 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1440_ssrc_skywalker_x8.hil b/ROMFS/px4fmu_common/init.d/airframes/1440_ssrc_skywalker_x8.hil index 1479ac19c747..0b8f1cf2f2fd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1440_ssrc_skywalker_x8.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1440_ssrc_skywalker_x8.hil @@ -21,13 +21,21 @@ param set-default CA_SV_CS1_TRQ_P 0.5 param set-default CA_SV_CS1_TRQ_R 0.5 # HITL PWM functions -param set-default HIL_ACT_FUNC1 201 -param set-default HIL_ACT_DIS1 500 -param set-default HIL_ACT_FUNC2 202 -param set-default HIL_ACT_DIS2 500 -param set-default HIL_ACT_FUNC4 101 -param set-default HIL_ACT_MIN4 150 -param set-default HIL_ACT_MAX4 1000 +param set PWM_MAIN_FUNC1 201 +param set PWM_MAIN_FUNC2 202 +param set PWM_MAIN_FUNC4 101 + +param set PWM_MAIN_MIN1 0 +param set PWM_MAIN_MIN2 0 +param set PWM_MAIN_MIN4 150 + +param set PWM_MAIN_MAX1 1000 +param set PWM_MAIN_MAX2 1000 +param set PWM_MAIN_MAX4 3000 + +param set PWM_MAIN_DIS1 500 +param set PWM_MAIN_DIS2 500 +param set PWM_MAIN_DIS4 0 # Airspeed parameters param set-default ASPD_PRIMARY 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor b/ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor index 59ffc5030294..8c51a35b4c4a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor +++ b/ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor @@ -51,12 +51,6 @@ param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 -# HITL PWM functions -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 - # Increase velocity controller P gain param set-default MPC_XY_VEL_P_ACC 2.4 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3e9e98706795..eed5b59d58de 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -355,11 +355,6 @@ else # if param greater SYS_HITL 0 then - if ! pwm_out_sim start -m hil - then - tune_control play error - fi - sensors start -h commander start -h # disable GPS diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index bdc4dd4362a2..db358c3e4d0e 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -5,7 +5,6 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_TONE_ALARM=y -CONFIG_DRIVERS_PWM_OUT_SIM=y CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y diff --git a/boards/ssrc/saluki-nxp93 b/boards/ssrc/saluki-nxp93 index 2eebb46f230b..2227541aac6b 160000 --- a/boards/ssrc/saluki-nxp93 +++ b/boards/ssrc/saluki-nxp93 @@ -1 +1 @@ -Subproject commit 2eebb46f230b92314c1578a232090db42f06b2dd +Subproject commit 2227541aac6bb61abd9396af598edda618ed7a3a diff --git a/boards/ssrc/saluki-pi b/boards/ssrc/saluki-pi index 83bb524c97fe..7dab5c1fdf15 160000 --- a/boards/ssrc/saluki-pi +++ b/boards/ssrc/saluki-pi @@ -1 +1 @@ -Subproject commit 83bb524c97fead9d41bd282927ba3805ab965bd1 +Subproject commit 7dab5c1fdf156cf91a87d2b63eeaeb4ec5d8cbed diff --git a/boards/ssrc/saluki-v1 b/boards/ssrc/saluki-v1 index 607841505a43..bac0ef50df7e 160000 --- a/boards/ssrc/saluki-v1 +++ b/boards/ssrc/saluki-v1 @@ -1 +1 @@ -Subproject commit 607841505a43687b8cfd37af909eb91cc5f332b7 +Subproject commit bac0ef50df7efedff8baef9e09a3a8cf34de98cc diff --git a/boards/ssrc/saluki-v2 b/boards/ssrc/saluki-v2 index 439248e6adb9..ff9cf2d81743 160000 --- a/boards/ssrc/saluki-v2 +++ b/boards/ssrc/saluki-v2 @@ -1 +1 @@ -Subproject commit 439248e6adb903e826b7207a9049613cb580a9ea +Subproject commit ff9cf2d817435232b21ec3be13f9d36aab4ffba8 diff --git a/boards/ssrc/saluki-v3 b/boards/ssrc/saluki-v3 index 41a014d94e4f..1c5332760fc5 160000 --- a/boards/ssrc/saluki-v3 +++ b/boards/ssrc/saluki-v3 @@ -1 +1 @@ -Subproject commit 41a014d94e4f58460f5b8b5ac608e48393822bf7 +Subproject commit 1c5332760fc52d373c25a438c7b0e58278078bf0 diff --git a/src/drivers/pwm_esc/pwm_esc.cpp b/src/drivers/pwm_esc/pwm_esc.cpp index b12a9e1d2970..348b25d8a45d 100644 --- a/src/drivers/pwm_esc/pwm_esc.cpp +++ b/src/drivers/pwm_esc/pwm_esc.cpp @@ -31,9 +31,9 @@ * ****************************************************************************/ -/** +/**const uint32_t reversible_outputs = _mixing_output.reversibleOutputs(); * @file pwm_esc.cpp - * Driver for the NuttX PWM driver controleed escs + * Driver for the NuttX PWM driver controlled escs * */ @@ -352,19 +352,26 @@ PWMESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigne } else { // In hitl, publish actuator_outputs_sim // Only publish once we receive actuator_controls (important for lock-step to work correctly) - actuator_outputs_s actuator_outputs_sim{}; + if (num_control_groups_updated > 0) { + actuator_outputs_s actuator_outputs_sim{}; + actuator_outputs_sim.noutputs = num_outputs; + + const uint32_t reversible_outputs = _mixing_output.reversibleOutputs(); + for (int i = 0; i < (int)num_outputs; i++) { uint16_t disarmed = _mixing_output.disarmedValue(i); uint16_t min = _mixing_output.minValue(i); uint16_t max = _mixing_output.maxValue(i); OutputFunction function = _mixing_output.outputFunction(i); + bool is_reversible = reversible_outputs & (1u << i); float output = outputs[i]; - if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)) { - // Scale motors to [0, 1] + if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax + && !is_reversible)) { + // Scale non-reversible motors to [0, 1] actuator_outputs_sim.output[i] = (output - disarmed) / (max - disarmed); } else { @@ -633,7 +640,7 @@ int PWMESC::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Driver for PWM outputs. +Driver for PWM outputs. Used also in HITL mode. )DESCR_STR"); diff --git a/src/lib/mixer_module/mixer_module_tests.cpp b/src/lib/mixer_module/mixer_module_tests.cpp index fe6cb7977293..1ac0b2a0f079 100644 --- a/src/lib/mixer_module/mixer_module_tests.cpp +++ b/src/lib/mixer_module/mixer_module_tests.cpp @@ -44,11 +44,7 @@ #include "mixer_module.hpp" -#if defined(CONFIG_ARCH_BOARD_PX4_SITL) #define PARAM_PREFIX "PWM_MAIN" -#else -#define PARAM_PREFIX "HIL_ACT" -#endif static constexpr int max_num_outputs = 8; diff --git a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp index 01d317b66975..72bd6a4c2ea1 100644 --- a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp +++ b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp @@ -79,8 +79,16 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) { char param_name[17]; - snprintf(param_name, sizeof(param_name), "%s_%s%d", "HIL_ACT", "FUNC", i + 1); - param_get(param_find(param_name), &_output_functions[i]); + snprintf(param_name, sizeof(param_name), "%s_%s%d", "PWM_MAIN", "FUNC", i + 1); + param_t param_handle = param_find(param_name); + + if (param_handle == PARAM_INVALID) { + _output_functions[i] = 0; + continue; + + } else { + param_get(param_handle, &_output_functions[i]); + } } } diff --git a/src/modules/simulation/pwm_out_sim/CMakeLists.txt b/src/modules/simulation/pwm_out_sim/CMakeLists.txt index 2802df3874be..f97d79df5b83 100644 --- a/src/modules/simulation/pwm_out_sim/CMakeLists.txt +++ b/src/modules/simulation/pwm_out_sim/CMakeLists.txt @@ -31,19 +31,13 @@ # ############################################################################ -if(${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL} STREQUAL "px4_sitl") - set(module_config "module_sim.yaml") -else() - set(module_config "module_hil.yaml") -endif() - px4_add_module( MODULE modules__simulation__pwm_out_sim MAIN pwm_out_sim SRCS PWMSim.cpp MODULE_CONFIG - ${module_config} + module.yaml DEPENDS mixer_module ) diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index 39434786da99..f367939e5db6 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -41,38 +41,19 @@ #include -PWMSim::PWMSim(bool hil_mode_enabled) : +PWMSim::PWMSim() : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) { - int32_t use_hitl = 0; - param_get(param_find("SYS_HITL"), &use_hitl); - - if (use_hitl) { - for (int i = 0; i < MAX_ACTUATORS; ++i) { - char param_name[17]; - snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "MIN", i + 1); - param_get(param_find(param_name), &_pwm_min[i]); - snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "MAX", i + 1); - param_get(param_find(param_name), &_pwm_max[i]); - snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "DIS", i + 1); - param_get(param_find(param_name), &_pwm_disarmed[i]); - } - - } else { - // SITL gazebo classic case - for (int i = 0; i < MAX_ACTUATORS; ++i) { - _pwm_min[i] = PWM_SIM_PWM_MIN_MAGIC; - _pwm_max[i] = PWM_SIM_PWM_MAX_MAGIC; - _pwm_disarmed[i] = PWM_SIM_DISARMED_MAGIC; - } - - _mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC); - _mixing_output.setAllFailsafeValues(PWM_SIM_FAILSAFE_MAGIC); - _mixing_output.setAllMinValues(PWM_SIM_PWM_MIN_MAGIC); - _mixing_output.setAllMaxValues(PWM_SIM_PWM_MAX_MAGIC); + for (int i = 0; i < MAX_ACTUATORS; ++i) { + _pwm_min[i] = PWM_SIM_PWM_MIN_MAGIC; + _pwm_max[i] = PWM_SIM_PWM_MAX_MAGIC; + _pwm_disarmed[i] = PWM_SIM_DISARMED_MAGIC; } - _mixing_output.setIgnoreLockdown(hil_mode_enabled); + _mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC); + _mixing_output.setAllFailsafeValues(PWM_SIM_FAILSAFE_MAGIC); + _mixing_output.setAllMinValues(PWM_SIM_PWM_MIN_MAGIC); + _mixing_output.setAllMaxValues(PWM_SIM_PWM_MAX_MAGIC); } PWMSim::~PWMSim() @@ -145,24 +126,7 @@ void PWMSim::Run() int PWMSim::task_spawn(int argc, char *argv[]) { - bool hil_mode = false; - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "m:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'm': - hil_mode = strcmp(myoptarg, "hil") == 0; - break; - - default: - return print_usage("unrecognized flag"); - } - } - - PWMSim *instance = new PWMSim(hil_mode); + PWMSim *instance = new PWMSim(); if (!instance) { PX4_ERR("alloc failed"); @@ -202,15 +166,14 @@ Driver for simulated PWM outputs. Its only function is to take `actuator_motors` and `actuator_servos` uORB messages, mix them with any loaded mixer and output the result to the -`actuator_output` uORB topic. +`actuator_outputs_sim` uORB topic. -It is used in SITL and HITL. +It is used only in SITL. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("pwm_out_sim", "driver"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module"); - PRINT_MODULE_USAGE_PARAM_STRING('m', "sim", "hil|sim", "Mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index e9bcf323ee6b..83df8e3e69b3 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp @@ -44,18 +44,14 @@ #include #include -#if defined(CONFIG_ARCH_BOARD_PX4_SITL) #define PARAM_PREFIX "PWM_MAIN" -#else -#define PARAM_PREFIX "HIL_ACT" -#endif using namespace time_literals; class PWMSim : public ModuleBase, public OutputModuleInterface { public: - PWMSim(bool hil_mode_enabled); + PWMSim(); ~PWMSim() override; /** @see ModuleBase */ diff --git a/src/modules/simulation/pwm_out_sim/module_sim.yaml b/src/modules/simulation/pwm_out_sim/module.yaml similarity index 100% rename from src/modules/simulation/pwm_out_sim/module_sim.yaml rename to src/modules/simulation/pwm_out_sim/module.yaml diff --git a/src/modules/simulation/pwm_out_sim/module_hil.yaml b/src/modules/simulation/pwm_out_sim/module_hil.yaml deleted file mode 100644 index f80ef6b97ce5..000000000000 --- a/src/modules/simulation/pwm_out_sim/module_hil.yaml +++ /dev/null @@ -1,13 +0,0 @@ - -module_name: HIL -actuator_output: - show_subgroups_if: 'SYS_HITL>0' - output_groups: - - param_prefix: HIL_ACT - channel_label: Channel - standard_params: - disarmed: { min: 0, max: 3500, default: 0 } - min: { min: 0, max: 1000, default: 0 } - max: { min: 0, max: 3500, default: 1000 } - failsafe: { min: 0, max: 3500 } - num_channels: 16 diff --git a/ssrc_config/config_hitl_eth_gzsim_fw.txt b/ssrc_config/config_hitl_eth_gzsim_fw.txt index 9ae79fdaa53c..49fe7f29b0d7 100644 --- a/ssrc_config/config_hitl_eth_gzsim_fw.txt +++ b/ssrc_config/config_hitl_eth_gzsim_fw.txt @@ -28,47 +28,30 @@ param set COM_RC_IN_MODE 1 # EKF2 param set EKF2_MULTI_IMU 1 +# Disable ESC arming check +param set FD_ESCS_EN 0 + #################################### # specific fw HITL configuration #################################### -# Override airframe defaults -param set ASPD_DO_CHECKS 7 -param set FW_LAUN_DETCN_ON 0 - # HITL PWM functions -param set HIL_ACT_FUNC1 201 -param set HIL_ACT_DIS1 500 -param set HIL_ACT_FUNC2 202 -param set HIL_ACT_DIS2 500 -param set HIL_ACT_FUNC4 101 -param set HIL_ACT_MIN4 150 -param set HIL_ACT_MAX4 3000 - -# Control allocator parameters -param set CA_AIRFRAME 1 -param set CA_ROTOR_COUNT 1 -param set CA_SV_CS_COUNT 2 -param set CA_SV_CS0_TYPE 5 -param set CA_SV_CS0_TRQ_P 0.5 -param set CA_SV_CS0_TRQ_R -0.5 -param set CA_SV_CS1_TYPE 6 -param set CA_SV_CS1_TRQ_P 0.5 -param set CA_SV_CS1_TRQ_R 0.5 +# TODO: adapt gz model to remove custom HITL min, max and disarmed limit +param set PWM_MAIN_MIN1 0 +param set PWM_MAIN_MIN2 0 -# Airspeed parameters -param set ASPD_PRIMARY 1 +param set PWM_MAIN_MAX1 1000 +param set PWM_MAIN_MAX2 1000 +param set PWM_MAIN_MAX4 3000 -# Maximum landing slope angle in deg -param set FW_LND_ANG 8.0 +param set PWM_MAIN_DIS1 500 +param set PWM_MAIN_DIS2 500 +param set PWM_MAIN_DIS4 0 -# RC loss failsafe to HOLD mode -param set COM_RC_IN_MODE 1 - -# Maximum manual roll angle -param set FW_MAN_R_MAX 60.0 +# Change airspeed checks +param set ASPD_DO_CHECKS 7 -# Fixed wing control +# FW control # Pitch rate param set FW_PR_P 0.9 param set FW_PR_FF 0.5 @@ -94,13 +77,9 @@ param set FW_T_CLMB_MAX 8.0 param set FW_T_SINK_MAX 2.7 param set FW_T_SINK_MIN 2.2 -# Navigation -param set NAV_ACC_RAD 15.0 -param set NAV_DLL_ACT 2 - -# Misc +# FW takeoff / landing param set RTL_RETURN_ALT 30.0 param set RTL_DESCEND_ALT 30.0 param set FW_LND_USETER 0 -param set RWTO_TKOFF 1 -param set FD_ESCS_EN 0 +param set FW_LAUN_DETCN_ON 0 +param set RWTO_TKOFF 1 \ No newline at end of file diff --git a/ssrc_config/config_hitl_eth_gzsim_mc.txt b/ssrc_config/config_hitl_eth_gzsim_mc.txt index 5ed5e0e67266..c66e101915db 100644 --- a/ssrc_config/config_hitl_eth_gzsim_mc.txt +++ b/ssrc_config/config_hitl_eth_gzsim_mc.txt @@ -31,41 +31,11 @@ param set EKF2_MULTI_IMU 1 # specific mc HITL configuration #################################### -# HITL PWM functions -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 CA_ROTOR0_PX 0.175 -param set-default CA_ROTOR0_PY 0.175 -param set-default CA_ROTOR0_KM 0.05 - -param set-default CA_ROTOR1_PX -0.175 -param set-default CA_ROTOR1_PY -0.175 -param set-default CA_ROTOR1_KM 0.05 - -param set-default CA_ROTOR2_PX 0.175 -param set-default CA_ROTOR2_PY -0.175 -param set-default CA_ROTOR2_KM -0.05 - -param set-default CA_ROTOR3_PX -0.175 -param set-default CA_ROTOR3_PY 0.175 -param set-default CA_ROTOR3_KM -0.05 - -# extra -param set-default MPC_THR_HOVER 0.60 param set COM_RCL_EXCEPT 4 param set NAV_DLL_ACT 0 param set NAV_RCL_ACT 0 param set MAV_0_BROADCAST 1 param set IMU_GYRO_CUTOFF 60 param set IMU_DGYRO_CUTOFF 30 -param set MC_ROLLRATE_P 0.14 -param set MC_PITCHRATE_P 0.14 -param set MC_ROLLRATE_I 0.3 -param set MC_PITCHRATE_I 0.3 -param set MC_ROLLRATE_D 0.004 -param set MC_PITCHRATE_D 0.004 param set BAT_N_CELLS 4 param set SDLOG_MODE 0 \ No newline at end of file diff --git a/ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt b/ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt index 1093870464a7..28e5e9ff7c09 100644 --- a/ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt +++ b/ssrc_config/config_hitl_eth_gzsim_vtol_sm.txt @@ -28,148 +28,46 @@ param set COM_RC_IN_MODE 1 # EKF2 param set EKF2_MULTI_IMU 1 +# Bypass some ESC checks +param set FD_ACT_EN 0 +param set FD_ACT_MOT_TOUT 500 +param set FD_ESCS_EN 0 + #################################### # specific vtol HITL configuration #################################### # HITL PWM functions -param set HIL_ACT_FUNC1 101 -param set HIL_ACT_MIN1 10 -param set HIL_ACT_MAX1 1500 -param set HIL_ACT_FUNC2 102 -param set HIL_ACT_MIN2 10 -param set HIL_ACT_MAX2 1500 -param set HIL_ACT_FUNC3 103 -param set HIL_ACT_MIN3 10 -param set HIL_ACT_MAX3 1500 -param set HIL_ACT_FUNC4 104 -param set HIL_ACT_MIN4 10 -param set HIL_ACT_MAX4 1500 -param set HIL_ACT_FUNC5 201 -param set HIL_ACT_DIS5 500 -param set HIL_ACT_FUNC6 202 -param set HIL_ACT_DIS6 500 -param set HIL_ACT_FUNC7 203 -param set HIL_ACT_DIS7 500 -param set HIL_ACT_FUNC8 204 -param set HIL_ACT_DIS8 500 -param set HIL_ACT_FUNC9 205 -param set HIL_ACT_DIS9 500 -param set HIL_ACT_FUNC10 105 -param set HIL_ACT_MIN10 0 -param set HIL_ACT_MAX10 3500 - -# Control allocator parameters -param set CA_AIRFRAME 2 -param set CA_ROTOR_COUNT 5 -param set CA_ROTOR0_PX 0.37 -param set CA_ROTOR0_PY 0.42 -param set CA_ROTOR1_PX -0.41 -param set CA_ROTOR1_PY -0.42 -param set CA_ROTOR2_PX 0.37 -param set CA_ROTOR2_PY -0.42 -param set CA_ROTOR2_KM -0.05 -param set CA_ROTOR3_PX -0.41 -param set CA_ROTOR3_PY 0.42 -param set CA_ROTOR3_KM -0.05 -param set CA_ROTOR4_AX 1.0 -param set CA_ROTOR4_AZ 0.0 -param set CA_ROTOR4_PX 0.0 -param set CA_ROTOR4_KM 0.05 - -param set CA_SV_CS_COUNT 5 -param set CA_SV_CS0_TYPE 1 -param set CA_SV_CS0_TRQ_R -0.5 -param set CA_SV_CS1_TYPE 2 -param set CA_SV_CS1_TRQ_R 0.5 -param set CA_SV_CS2_TYPE 3 -param set CA_SV_CS2_TRQ_P 1.0 -param set CA_SV_CS2_TRIM 0.1 -param set CA_SV_CS3_TYPE 3 -param set CA_SV_CS3_TRQ_P 1.0 -param set CA_SV_CS3_TRIM 0.1 -param set CA_SV_CS4_TYPE 4 -param set CA_SV_CS4_TRQ_Y 1.0 - -# Fixed wing specific -param set FW_RR_IMAX 0.4000 -param set FW_YR_IMAX 0.4000 - -param set FW_YR_P 0.2 -param set FW_YR_I 0.01 - -param set FW_RR_FF 0.5 -param set FW_RR_P 0.05 -param set FW_RR_I 0.1 -param set FW_RR_IMAX 0.4 - -param set FW_R_LIM 50.0 -param set FW_R_RMAX 70.0 -param set FW_R_TC 0.4 - -param set FW_PR_FF 0.5 -param set FW_PR_I 0.1 -param set FW_PR_IMAX 0.4 - -param set FW_P_LIM_MAX 30.0 -param set FW_P_LIM_MIN -30.0 - -# Airspeed parameters -param set ASPD_PRIMARY 1 -param set FW_AIRSPD_MAX 25.0 -param set FW_AIRSPD_MIN 15.0 -param set FW_AIRSPD_STALL 12.0 -param set FW_AIRSPD_TRIM 18.0 - -# VTOL specific -# VTOL type -param set VT_TYPE 2 - -# Airspeed at which we can start blending both fw and mc controls. -param set VT_ARSP_BLEND 8.0 - -# Airspeed at which we can switch to fw mode -param set VT_ARSP_TRANS 14.0 - -# Back-transition duration -param set VT_B_TRANS_DUR 10.0 -param set VT_B_TRANS_RAMP 3.0 - -# Transition duration -param set VT_F_TRANS_DUR 6.0 -param set VT_F_TRANS_THR 1.0 - -# VTOL takeoff -param set MIS_TAKEOFF_ALT 10.0 -param set VTO_LOITER_ALT 10.0 - -param set MC_AIRMODE 0 -param set MC_ROLLRATE_P 0.14 -param set MC_ROLLRATE_I 0.2 -param set MC_PITCHRATE_P 0.14 -param set MC_PITCHRATE_I 0.2 -param set MC_YAW_P 2.0 -param set MC_YAW_WEIGHT 0.5 -param set MC_YAWRATE_P 0.2 -param set MC_YAWRATE_I 0.1 -param set MC_YAWRATE_MAX 120.0 - -param set MPC_XY_P 0.95 -param set MPC_XY_VEL_P_ACC 1.8 -param set MPC_XY_VEL_I_ACC 0.4 -param set MPC_XY_VEL_D_ACC 0.2 -param set MPC_YAW_MODE 4 - -param set NAV_ACC_RAD 10.0 - -# QuadChute altitude (transition to quad mode as a failsafe) -param set VT_FW_MIN_ALT 5.0 - -# QuadChute angle limits -param set VT_FW_QC_P 35 -param set VT_FW_QC_R 60 - -# Others -param set FD_ACT_EN 0 -param set FD_ACT_MOT_TOUT 500 -param set FD_ESCS_EN 0 +# TODO: adapt gz model to remove custom HITL min, max and disarmed limit +param set PWM_MAIN_MIN1 10 +param set PWM_MAIN_MIN2 10 +param set PWM_MAIN_MIN3 10 +param set PWM_MAIN_MIN4 10 +param set PWM_MAIN_MIN5 0 +param set PWM_MAIN_MIN6 0 +param set PWM_MAIN_MIN7 0 +param set PWM_MAIN_MIN8 0 +param set PWM_MAIN_MIN9 0 +param set PWM_MAIN_MIN10 0 + +param set PWM_MAIN_MAX1 1500 +param set PWM_MAIN_MAX2 1500 +param set PWM_MAIN_MAX3 1500 +param set PWM_MAIN_MAX4 1500 +param set PWM_MAIN_MAX5 1000 +param set PWM_MAIN_MAX6 1000 +param set PWM_MAIN_MAX7 1000 +param set PWM_MAIN_MAX8 1000 +param set PWM_MAIN_MAX9 1000 +param set PWM_MAIN_MAX10 3500 + +param set PWM_MAIN_DIS1 0 +param set PWM_MAIN_DIS2 0 +param set PWM_MAIN_DIS3 0 +param set PWM_MAIN_DIS4 0 +param set PWM_MAIN_DIS5 500 +param set PWM_MAIN_DIS6 500 +param set PWM_MAIN_DIS7 500 +param set PWM_MAIN_DIS8 500 +param set PWM_MAIN_DIS9 500 +param set PWM_MAIN_DIS10 0 diff --git a/ssrc_config/config_hitl_eth_gzsim_vtol_sv.txt b/ssrc_config/config_hitl_eth_gzsim_vtol_sv.txt index fd6782354c37..3fc1eb7382e7 100644 --- a/ssrc_config/config_hitl_eth_gzsim_vtol_sv.txt +++ b/ssrc_config/config_hitl_eth_gzsim_vtol_sv.txt @@ -33,27 +33,41 @@ param set EKF2_MULTI_IMU 1 #################################### # HITL PWM functions -param set HIL_ACT_FUNC1 101 -param set HIL_ACT_MIN1 10 -param set HIL_ACT_MAX1 1500 -param set HIL_ACT_FUNC2 102 -param set HIL_ACT_MIN2 10 -param set HIL_ACT_MAX2 1500 -param set HIL_ACT_FUNC3 103 -param set HIL_ACT_MIN3 10 -param set HIL_ACT_MAX3 1500 -param set HIL_ACT_FUNC4 104 -param set HIL_ACT_MIN4 10 -param set HIL_ACT_MAX4 1500 -param set HIL_ACT_FUNC5 105 -param set HIL_ACT_MIN5 0 -param set HIL_ACT_MAX5 3500 -param set HIL_ACT_FUNC6 201 -param set HIL_ACT_DIS6 500 -param set HIL_ACT_FUNC7 202 -param set HIL_ACT_DIS7 500 -param set HIL_ACT_FUNC8 203 -param set HIL_ACT_DIS8 500 +param set PWM_MAIN_FUNC1 101 +param set PWM_MAIN_FUNC2 102 +param set PWM_MAIN_FUNC3 103 +param set PWM_MAIN_FUNC4 104 +param set PWM_MAIN_FUNC5 105 +param set PWM_MAIN_FUNC6 201 +param set PWM_MAIN_FUNC7 202 +param set PWM_MAIN_FUNC8 203 + +param set PWM_MAIN_MIN1 10 +param set PWM_MAIN_MIN2 10 +param set PWM_MAIN_MIN3 10 +param set PWM_MAIN_MIN4 10 +param set PWM_MAIN_MIN5 0 +param set PWM_MAIN_MIN6 0 +param set PWM_MAIN_MIN7 0 +param set PWM_MAIN_MIN8 0 + +param set PWM_MAIN_MAX1 1500 +param set PWM_MAIN_MAX2 1500 +param set PWM_MAIN_MAX3 1500 +param set PWM_MAIN_MAX4 1500 +param set PWM_MAIN_MAX5 3500 +param set PWM_MAIN_MAX6 1000 +param set PWM_MAIN_MAX7 1000 +param set PWM_MAIN_MAX8 1000 + +param set PWM_MAIN_DIS1 0 +param set PWM_MAIN_DIS2 0 +param set PWM_MAIN_DIS3 0 +param set PWM_MAIN_DIS4 0 +param set PWM_MAIN_DIS5 0 +param set PWM_MAIN_DIS6 500 +param set PWM_MAIN_DIS7 500 +param set PWM_MAIN_DIS8 500 # Control allocator parameters