From 394aff6535983c93bf8484b89e17d153bbeff29d Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Mon, 5 Aug 2019 14:14:45 -0300 Subject: [PATCH] SITL: add deadzones to Sub thrusters PWM signals --- libraries/SITL/SIM_Submarine.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/SITL/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp index a15358ec7f..c90f0f4ad7 100644 --- a/libraries/SITL/SIM_Submarine.cpp +++ b/libraries/SITL/SIM_Submarine.cpp @@ -76,8 +76,10 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a Thruster t = thrusters[i]; int16_t pwm = input.servos[t.servo]; float output = 0; - if (pwm < 2000 && pwm > 1000) { - output = (pwm - 1500) / 400.0; // range -1~1 + // if valid pwm and not in the esc deadzone + // TODO: extract deadzone from parameters/vehicle code + if (pwm < 2000 && pwm > 1000 && (pwm < 1475 || pwm > 1525)) { + output = (pwm - 1500) / 400.0; // range -1~1 } // 2.5 scalar for approximate real-life performance of T200 thruster