SRV_Channel: pwm_from_angle: return trim for 0 high_out

This commit is contained in:
Iampete1 2022-10-20 17:01:58 +01:00 committed by Andrew Tridgell
parent 623501d0d5
commit 5382083452

View File

@ -99,6 +99,9 @@ uint16_t SRV_Channel::pwm_from_range(float scaled_value) const
// convert a -angle_max..angle_max to a pwm
uint16_t SRV_Channel::pwm_from_angle(float scaled_value) const
{
if (high_out == 0) {
return servo_trim;
}
if (reversed) {
scaled_value = -scaled_value;
}