From c44b8cf73af66b079d53c33f77d5125f2556dfc0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 29 Dec 2021 14:40:20 +1100 Subject: [PATCH] AP_Motors: fixed PWM_TYPE range for quadplanes when motors don't start at SERVO1 we were applying the range to the wrong output. This allows users to control the PWM of individual motors precisely for quadplanes where calibration of motors is difficult --- libraries/AP_Motors/AP_Motors_Class.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 04f8e3f0bb..fb358031be 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -112,13 +112,13 @@ void AP_Motors::rc_write_angle(uint8_t chan, int16_t angle_cd) /* set frequency of a set of channels */ -void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz) +void AP_Motors::rc_set_freq(uint32_t motor_mask, uint16_t freq_hz) { if (freq_hz > 50) { - _motor_fast_mask |= mask; + _motor_fast_mask |= motor_mask; } - mask = motor_mask_to_srv_channel_mask(mask); + const uint32_t mask = motor_mask_to_srv_channel_mask(motor_mask); hal.rcout->set_freq(mask, freq_hz); switch (pwm_type(_pwm_type.get())) { @@ -152,9 +152,9 @@ void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz) this is a motor output type for multirotors which honours the SERVOn_MIN/MAX values per channel */ - _motor_pwm_range_mask |= mask; + _motor_pwm_range_mask |= motor_mask; for (uint8_t i=0; i<16; i++) { - if ((1U<