Skip to content

Commit

Permalink
Extra fix for SITL classic
Browse files Browse the repository at this point in the history
  • Loading branch information
haitomatic committed Sep 8, 2024
1 parent 02738c8 commit 572c04c
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 10 deletions.
36 changes: 27 additions & 9 deletions src/modules/simulation/pwm_out_sim/PWMSim.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
}
}
7 changes: 6 additions & 1 deletion src/modules/simulation/pwm_out_sim/PWMSim.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,11 @@ class PWMSim : public ModuleBase<PWMSim>, 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] {};
Expand All @@ -87,4 +92,4 @@ class PWMSim : public ModuleBase<PWMSim>, 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")};
};
};

0 comments on commit 572c04c

Please sign in to comment.