SITL: add deadzones to Sub thrusters PWM signals

This commit is contained in:
Willian Galvani 2019-08-05 14:14:45 -03:00 committed by Jacob Walser
parent 1e2340fbb4
commit 394aff6535
1 changed files with 4 additions and 2 deletions

View File

@ -76,8 +76,10 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a
Thruster t = thrusters[i]; Thruster t = thrusters[i];
int16_t pwm = input.servos[t.servo]; int16_t pwm = input.servos[t.servo];
float output = 0; float output = 0;
if (pwm < 2000 && pwm > 1000) { // if valid pwm and not in the esc deadzone
output = (pwm - 1500) / 400.0; // range -1~1 // 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 // 2.5 scalar for approximate real-life performance of T200 thruster