From 18ec5609b736ebd60b17e37681bdfa668784e5ca Mon Sep 17 00:00:00 2001 From: haitomatic Date: Thu, 5 Sep 2024 07:28:14 +0000 Subject: [PATCH] Fix classic SITL after being broken by PR#743 --- src/modules/simulation/pwm_out_sim/PWMSim.cpp | 32 ++++++++++++++----- src/modules/simulation/pwm_out_sim/PWMSim.hpp | 5 +++ 2 files changed, 29 insertions(+), 8 deletions(-) diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index 6e8836bde638..61eff7203cb1 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -44,14 +44,30 @@ PWMSim::PWMSim(bool hil_mode_enabled) : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) { - 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]); + 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); } _mixing_output.setIgnoreLockdown(hil_mode_enabled); diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index a24ae9b526e3..e9bcf323ee6b 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp @@ -76,6 +76,11 @@ class PWMSim : public ModuleBase, public OutputModuleInterface private: void Run() override; + static constexpr uint16_t PWM_SIM_DISARMED_MAGIC = 900; + static constexpr uint16_t PWM_SIM_FAILSAFE_MAGIC = 600; + static constexpr uint16_t PWM_SIM_PWM_MIN_MAGIC = 1000; + static constexpr uint16_t PWM_SIM_PWM_MAX_MAGIC = 2000; + int32_t _pwm_min[MAX_ACTUATORS] {}; int32_t _pwm_max[MAX_ACTUATORS] {}; int32_t _pwm_disarmed[MAX_ACTUATORS] {};