AP_Motors: ensure OneShot125 is within 125 to 250usec

This commit is contained in:
Andrew Tridgell 2016-04-16 18:52:12 +10:00
parent 3acfe8bea3
commit 6df4d11d3f

View File

@ -82,6 +82,16 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
if (_pwm_type == PWM_TYPE_ONESHOT125) {
// OneShot125 uses a PWM range from 125 to 250 usec
pwm /= 8;
/*
OneShot125 ESCs can be confused by pulses below 125 or above
250, making them fail the pulse type auto-detection. This
happens at least with BLHeli
*/
if (pwm < 125) {
pwm = 125;
} else if (pwm > 250) {
pwm = 250;
}
}
hal.rcout->write(chan, pwm);
}