From ce87bb7e397541e7b1a6ced35fe21dd9d5981787 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Apr 2017 16:53:05 +1000 Subject: [PATCH] SRV_Channel: added set_output_pwm_chan() --- libraries/SRV_Channel/SRV_Channel.h | 3 +++ libraries/SRV_Channel/SRV_Channels.cpp | 8 ++++++++ 2 files changed, 11 insertions(+) diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index fcdfac5aee..8138c4bed8 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -225,6 +225,9 @@ public: // set output value for a function channel as a pwm value on the first matching channel static void set_output_pwm_first(SRV_Channel::Aux_servo_function_t function, uint16_t value); + // set output value for a specific function channel as a pwm value + static void set_output_pwm_chan(uint8_t chan, uint16_t value); + // set output value for a function channel as a scaled value. This // calls calc_pwm() to also set the pwm value static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value); diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 0d6fa42dee..b69fe8dec1 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -176,3 +176,11 @@ void SRV_Channels::calc_pwm(void) channels[i].calc_pwm(functions[channels[i].function].output_scaled); } } + +// set output value for a specific function channel as a pwm value +void SRV_Channels::set_output_pwm_chan(uint8_t chan, uint16_t value) +{ + if (chan < NUM_SERVO_CHANNELS) { + channels[chan].set_output_pwm(value); + } +}