Fix classic SITL after being broken by PR#743

tiiuae · Sep 5, 2024 · 70b8bbf · 70b8bbf
1 parent bd2d6ae
commit 70b8bbf
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 7 deletions.
diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp
@@ -44,14 +44,33 @@
 PWMSim::PWMSim(bool hil_mode_enabled) :
 	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
 {
+	bool use_magic_numbers = false;
+
 	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]);
+		char min_param_name[17];
+		char max_param_name[17];
+		char disarmed_param_name[17];
+		snprintf(min_param_name, sizeof(min_param_name), "%s_%s%d", PARAM_PREFIX, "MIN", i + 1);
+		snprintf(max_param_name, sizeof(max_param_name), "%s_%s%d", PARAM_PREFIX, "MAX", i + 1);
+		snprintf(disarmed_param_name, sizeof(disarmed_param_name), "%s_%s%d", PARAM_PREFIX, "DIS", i + 1);
+
+		if (param_get(param_find(min_param_name), &_pwm_max[i]) == PX4_OK &&
+		    param_get(param_find(max_param_name), &_pwm_min[i]) == PX4_OK &&
+		    param_get(param_find(disarmed_param_name), &_pwm_disarmed[i]) == PX4_OK) {
+		} else {
+			// invalid parameters, set to magic numbers
+			_pwm_max[i] = PWM_SIM_PWM_MAX_MAGIC;
+			_pwm_min[i] = PWM_SIM_PWM_MIN_MAGIC;
+			_pwm_disarmed[i] = PWM_SIM_DISARMED_MAGIC;
+			use_magic_numbers = true;
+		}
+	}
+
+	if (use_magic_numbers) {
+		_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
@@ -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] {};