/// 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(); } } } }