diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index 6e8836bde638..16aed10e7df8 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -44,14 +44,32 @@ 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); @@ -201,4 +219,4 @@ It is used in SITL and HITL. extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[]) { return PWMSim::main(argc, argv); -} +} \ No newline at end of file diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index a24ae9b526e3..2a020f03dbaf 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] {}; @@ -87,4 +92,4 @@ class PWMSim : public ModuleBase, public OutputModuleInterface perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; -}; +}; \ No newline at end of file