/*
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;
}
