/// map a function to a servo channel and output it
void SRV_Channel::output_ch(void)
{
int8_t passthrough_from = -1;
// take care of special function cases
switch(function)
{
case k_manual: // manual
passthrough_from = ch_num;
break;
case k_rcin1 ... k_rcin16: // rc pass-thru
passthrough_from = int8_t(function - k_rcin1);
break;
}
if (passthrough_from != -1) {
// we are doing passthrough from input to output for this channel
RC_Channel *rc = RC_Channels::rc_channel(passthrough_from);
if (rc) {
if (SRV_Channels::passthrough_disabled()) {
output_pwm = rc->get_radio_trim();
} else {
output_pwm = rc->get_radio_in();
}
}
}
if (!(SRV_Channels::disabled_mask & (1U<<ch_num))) {
hal.rcout->write(ch_num, output_pwm);
}
}
/*
copy radio_in to radio_out for a channel mask
*/
void
SRV_Channels::copy_radio_in_out_mask(uint16_t mask)
{
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
if ((1U<<i) & mask) {
RC_Channel *rc = RC_Channels::rc_channel(channels[i].ch_num);
if (rc == nullptr) {
continue;
}
channels[i].set_output_pwm(rc->get_radio_in());
}
}
}
/*
copy radio_in to radio_out for a given function
*/
void
SRV_Channels::copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output)
{
if (!function_assigned(function)) {
return;
}
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
if (channels[i].function.get() == function) {
RC_Channel *rc = RC_Channels::rc_channel(channels[i].ch_num);
if (rc == nullptr) {
continue;
}
if (do_input_output) {
rc->read();
}
channels[i].set_output_pwm(rc->get_radio_in());
if (do_input_output) {
channels[i].output_ch();
}
}
}
}
