From 2a6e64e3582547c672867cac9a178d9a9c1d513a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 13 Mar 2016 10:03:56 +1100 Subject: [PATCH] HAL_PX4: send all channels to px4io this allows for 16 channel SBUS out --- libraries/AP_HAL_PX4/RCOutput.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp index 6a73dae03e..1d35176612 100644 --- a/libraries/AP_HAL_PX4/RCOutput.cpp +++ b/libraries/AP_HAL_PX4/RCOutput.cpp @@ -399,11 +399,10 @@ void PX4RCOutput::_timer_tick(void) if (_need_update && _pwm_fd != -1) { _need_update = false; perf_begin(_perf_rcout); - if (_max_channel <= _servo_count) { - ::write(_pwm_fd, _period, _max_channel*sizeof(_period[0])); - } else { - // we're using both sets of outputs - ::write(_pwm_fd, _period, _servo_count*sizeof(_period[0])); + // always send all outputs to first PWM device. This ensures that SBUS is properly updated in px4io + ::write(_pwm_fd, _period, _max_channel*sizeof(_period[0])); + if (_max_channel > _servo_count) { + // maybe send updates to alt_fd if (_alt_fd != -1 && _alt_servo_count > 0) { uint8_t n = _max_channel - _servo_count; if (n > _alt_servo_count) {