/* set servo_out and call calc_pwm() for a given function */ void RC_Channel_aux::set_servo_out(RC_Channel_aux::Aux_servo_function_t function, int16_t value) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { _aux_channels[i]->servo_out = value; _aux_channels[i]->calc_pwm(); _aux_channels[i]->output(); } } }
/* setup safety value for an auxiliary function type to a LimitValue */ void SRV_Channels::set_safety_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { const SRV_Channel &ch = channels[i]; if (ch.function.get() == function) { uint16_t pwm = ch.get_limit_pwm(limit); hal.rcout->set_safety_pwm(1U<<ch.ch_num, pwm); } } }
/* setup failsafe value for an auxiliary function type to a LimitValue */ void RC_Channel_aux::set_servo_failsafe(RC_Channel_aux::Aux_servo_function_t function, RC_Channel::LimitValue limit) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { const RC_Channel_aux *ch = _aux_channels[i]; if (ch && ch->function.get() == function) { uint16_t pwm = ch->get_limit_pwm(limit); hal.rcout->set_failsafe_pwm(1U<<ch->get_ch_out(), pwm); } } }
/* set radio_out for all channels matching the given function type, allow radio_trim to center servo */ void RC_Channel_aux::set_radio_trimmed(RC_Channel_aux::Aux_servo_function_t function, int16_t value) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { int16_t value2 = value - 1500 + _aux_channels[i]->get_radio_trim(); _aux_channels[i]->set_radio_out(constrain_int16(value2,_aux_channels[i]->get_radio_min(),_aux_channels[i]->get_radio_max())); _aux_channels[i]->output(); } } }
/* set radio_out for all channels matching the given function type trim the output assuming a 1500 center on the given value */ void SRV_Channels::set_output_pwm_trimmed(SRV_Channel::Aux_servo_function_t function, int16_t value) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { if (channels[i].function.get() == function) { int16_t value2 = value - 1500 + channels[i].get_trim(); channels[i].set_output_pwm(constrain_int16(value2,channels[i].get_output_min(),channels[i].get_output_max())); channels[i].output_ch(); } } }
/* set and save the trim value to radio_in for all channels matching the given function type */ void RC_Channel_aux::set_radio_trim(RC_Channel_aux::Aux_servo_function_t function) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { if (_aux_channels[i]->radio_in != 0) { _aux_channels[i]->radio_trim = _aux_channels[i]->radio_in; _aux_channels[i]->radio_trim.save(); } } } }
// find first channel that a function is assigned to bool SRV_Channels::find_channel(SRV_Channel::Aux_servo_function_t function, uint8_t &chan) { if (!initialised) { update_aux_servo_function(); } if (!function_assigned(function)) { return false; } for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) { if (channels[i].function == function) { chan = channels[i].ch_num; return true; } } return false; }
// find first channel that a function is assigned to bool RC_Channel_aux::find_channel(RC_Channel_aux::Aux_servo_function_t function, uint8_t &chan) { if (!_initialised) { update_aux_servo_function(); } if (!function_assigned(function)) { return false; } for (uint8_t i=0; i<RC_AUX_MAX_CHANNELS; i++) { if (_aux_channels[i] && _aux_channels[i]->function == function) { chan = _aux_channels[i]->_ch_out; return true; } } return false; }
/* set servo_out and angle_min/max, then calc_pwm and output a value. This is used to move a AP_Mount servo */ void RC_Channel_aux::move_servo(RC_Channel_aux::Aux_servo_function_t function, int16_t value, int16_t angle_min, int16_t angle_max) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { _aux_channels[i]->set_servo_out(value); _aux_channels[i]->set_range(angle_min, angle_max); _aux_channels[i]->calc_pwm(); _aux_channels[i]->output(); } } }
/* copy radio_in to radio_out for a given function */ void RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::Aux_servo_function_t function, bool do_input_output) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { if (_aux_channels[i] && _aux_channels[i]->function.get() == function) { if (do_input_output) { _aux_channels[i]->input(); } _aux_channels[i]->radio_out = _aux_channels[i]->radio_in; if (do_input_output) { _aux_channels[i]->output(); } } } }
/* set radio output value for an auxiliary function type to a LimitValue */ void RC_Channel_aux::set_servo_limit(RC_Channel_aux::Aux_servo_function_t function, RC_Channel::LimitValue limit) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < RC_AUX_MAX_CHANNELS; i++) { RC_Channel_aux *ch = _aux_channels[i]; if (ch && ch->function.get() == function) { uint16_t pwm = ch->get_limit_pwm(limit); ch->radio_out = pwm; if (ch->function.get() == k_manual) { // in order for output_ch() to work for k_manual we // also have to override radio_in ch->radio_in = pwm; } } } }
/* set servo_out and angle_min/max, then calc_pwm and output a value. This is used to move a AP_Mount servo */ void SRV_Channels::move_servo(SRV_Channel::Aux_servo_function_t function, int16_t value, int16_t angle_min, int16_t angle_max) { if (!function_assigned(function)) { return; } if (angle_max <= angle_min) { return; } float v = float(value - angle_min) / float(angle_max - angle_min); v = constrain_float(v, 0.0f, 1.0f); for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { SRV_Channel &ch = channels[i]; if (ch.function.get() == function) { float v2 = ch.get_reversed()? (1-v) : v; uint16_t pwm = ch.servo_min + v2 * (ch.servo_max - ch.servo_min); ch.set_output_pwm(pwm); } } }
/* set radio output value for an auxiliary function type to a LimitValue */ void SRV_Channels::set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit) { if (!function_assigned(function)) { return; } for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { SRV_Channel &ch = channels[i]; if (ch.function.get() == function) { uint16_t pwm = ch.get_limit_pwm(limit); ch.set_output_pwm(pwm); if (ch.function.get() == SRV_Channel::k_manual) { RC_Channel *rc = RC_Channels::rc_channel(ch.ch_num); if (rc != nullptr) { // in order for output_ch() to work for k_manual we // also have to override radio_in rc->set_radio_in(pwm); } } } } }
/* 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(); } } } }
/* set the default channel an auxillary output function should be on */ bool RC_Channel_aux::set_aux_channel_default(RC_Channel_aux::Aux_servo_function_t function, uint8_t channel) { if (function_assigned(function)) { // already assigned return true; } for (uint8_t i=0; i<RC_AUX_MAX_CHANNELS; i++) { if (_aux_channels[i] && _aux_channels[i]->_ch_out == channel) { if (_aux_channels[i]->function != k_none) { hal.console->printf("Channel %u already assigned %u\n", (unsigned)channel, (unsigned)_aux_channels[i]->function); return false; } _aux_channels[i]->function.set(function); _aux_channels[i]->aux_servo_function_setup(); return true; } } hal.console->printf("AUX channel %u not available\n", (unsigned)channel); return false; }