diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp index 1a2e1541f2..e40fd027b0 100644 --- a/libraries/AP_HAL_PX4/RCOutput.cpp +++ b/libraries/AP_HAL_PX4/RCOutput.cpp @@ -462,8 +462,7 @@ void PX4RCOutput::push() _send_outputs(); if (_fast_channel_mask != 0) { // this is a crude way of triggering immediate output - set_freq(_fast_channel_mask, 400); - set_freq(_fast_channel_mask, 0); + _trigger_fast_output(); } } } @@ -475,4 +474,28 @@ void PX4RCOutput::_timer_tick(void) } } +/* + trigger immediate output on fast channels. This only works on FMU, + not IO. It takes advantage of the way the rate update works on FMU + to trigger a oneshot output + */ +void PX4RCOutput::_trigger_fast_output(void) +{ + uint32_t primary_mask = _fast_channel_mask & ((1UL<<_servo_count)-1); + uint32_t alt_mask = _fast_channel_mask >> _servo_count; + if (_alt_fd == -1) { + // we're on a FMU only board + if (primary_mask) { + set_freq_fd(_pwm_fd, primary_mask, 400); + set_freq_fd(_pwm_fd, primary_mask, 0); + } + } else { + // we're on a board with px4io + if (alt_mask) { + set_freq_fd(_alt_fd, alt_mask, 400); + set_freq_fd(_alt_fd, alt_mask, 0); + } + } +} + #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h index 7a1ef8a851..65658ba827 100644 --- a/libraries/AP_HAL_PX4/RCOutput.h +++ b/libraries/AP_HAL_PX4/RCOutput.h @@ -70,4 +70,6 @@ private: bool _corking; enum output_mode _output_mode = MODE_PWM_NORMAL; void _send_outputs(void); + + void _trigger_fast_output(); };