Browse Source

SRV_Channel: fixed output slew rate handling

this fixes a bug that happens when the RC output speed is much lower
than the main loop speed, such as with forward throttle for
quadplanes. We need to base the slew on the last sent output, not the
last value read back (at 50Hz) from the IO board, or we will slew the
channel at 6x to 8x slower than the correct rate.
master
Andrew Tridgell 8 years ago
parent
commit
29b06d2d4a
  1. 2
      libraries/SRV_Channel/SRV_Channel_aux.cpp

2
libraries/SRV_Channel/SRV_Channel_aux.cpp

@ -558,7 +558,7 @@ void SRV_Channels::limit_slew_rate(SRV_Channel::Aux_servo_function_t function, f
SRV_Channel &ch = channels[i]; SRV_Channel &ch = channels[i];
if (ch.function == function) { if (ch.function == function) {
ch.calc_pwm(functions[function].output_scaled); ch.calc_pwm(functions[function].output_scaled);
uint16_t last_pwm = hal.rcout->read(ch.ch_num); uint16_t last_pwm = hal.rcout->read_last_sent(ch.ch_num);
if (last_pwm == ch.output_pwm) { if (last_pwm == ch.output_pwm) {
continue; continue;
} }

Loading…
Cancel
Save