mirror of https://github.com/ArduPilot/ardupilot
910 lines
28 KiB
C++
910 lines
28 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
/*
|
|
SRV_Channel_aux.cpp - handling of servo auxillary functions
|
|
*/
|
|
#include "SRV_Channel.h"
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <RC_Channel/RC_Channel.h>
|
|
#include <AP_RCMapper/AP_RCMapper.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
/// 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 *c = rc().channel(passthrough_from);
|
|
if (c) {
|
|
if (SRV_Channels::passthrough_disabled()) {
|
|
output_pwm = c->get_radio_trim();
|
|
} else {
|
|
const int16_t radio_in = c->get_radio_in();
|
|
if (!ign_small_rcin_changes) {
|
|
output_pwm = radio_in;
|
|
previous_radio_in = radio_in;
|
|
} else {
|
|
// check if rc input value has changed by more than the deadzone
|
|
if (abs(radio_in - previous_radio_in) > c->get_dead_zone()) {
|
|
output_pwm = radio_in;
|
|
ign_small_rcin_changes = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (!(SRV_Channels::disabled_mask & (1U<<ch_num))) {
|
|
hal.rcout->write(ch_num, output_pwm);
|
|
}
|
|
}
|
|
|
|
/*
|
|
call output_ch() on all channels
|
|
*/
|
|
void SRV_Channels::output_ch_all(void)
|
|
{
|
|
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
|
channels[i].output_ch();
|
|
}
|
|
}
|
|
|
|
/*
|
|
return the current function for a channel
|
|
*/
|
|
SRV_Channel::Aux_servo_function_t SRV_Channels::channel_function(uint8_t channel)
|
|
{
|
|
if (channel < NUM_SERVO_CHANNELS) {
|
|
return (SRV_Channel::Aux_servo_function_t)channels[channel].function.get();
|
|
}
|
|
return SRV_Channel::k_none;
|
|
}
|
|
|
|
/*
|
|
setup a channels aux servo function
|
|
*/
|
|
void SRV_Channel::aux_servo_function_setup(void)
|
|
{
|
|
if (type_setup) {
|
|
return;
|
|
}
|
|
switch (function) {
|
|
case k_flap:
|
|
case k_flap_auto:
|
|
case k_egg_drop:
|
|
set_range(100);
|
|
break;
|
|
case k_heli_rsc:
|
|
case k_heli_tail_rsc:
|
|
case k_motor_tilt:
|
|
case k_boost_throttle:
|
|
case k_thrust_out:
|
|
set_range(1000);
|
|
break;
|
|
case k_aileron_with_input:
|
|
case k_elevator_with_input:
|
|
case k_aileron:
|
|
case k_elevator:
|
|
case k_dspoilerLeft1:
|
|
case k_dspoilerLeft2:
|
|
case k_dspoilerRight1:
|
|
case k_dspoilerRight2:
|
|
case k_rudder:
|
|
case k_steering:
|
|
case k_flaperon_left:
|
|
case k_flaperon_right:
|
|
case k_tiltMotorLeft:
|
|
case k_tiltMotorRight:
|
|
case k_elevon_left:
|
|
case k_elevon_right:
|
|
case k_vtail_left:
|
|
case k_vtail_right:
|
|
case k_roll_out:
|
|
case k_pitch_out:
|
|
case k_yaw_out:
|
|
set_angle(4500);
|
|
break;
|
|
case k_throttle:
|
|
case k_throttleLeft:
|
|
case k_throttleRight:
|
|
// fixed wing throttle
|
|
set_range(100);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/// setup the output range types of all functions
|
|
void SRV_Channels::update_aux_servo_function(void)
|
|
{
|
|
if (!channels) {
|
|
return;
|
|
}
|
|
function_mask.clearall();
|
|
|
|
for (uint8_t i = 0; i < SRV_Channel::k_nr_aux_servo_functions; i++) {
|
|
functions[i].channel_mask = 0;
|
|
}
|
|
|
|
// set auxiliary ranges
|
|
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
|
if ((uint8_t)channels[i].function.get() < SRV_Channel::k_nr_aux_servo_functions) {
|
|
channels[i].aux_servo_function_setup();
|
|
function_mask.set((uint8_t)channels[i].function.get());
|
|
functions[channels[i].function.get()].channel_mask |= 1U<<i;
|
|
}
|
|
}
|
|
initialised = true;
|
|
}
|
|
|
|
/// Should be called after the the servo functions have been initialized
|
|
void SRV_Channels::enable_aux_servos()
|
|
{
|
|
hal.rcout->set_default_rate(uint16_t(_singleton->default_rate.get()));
|
|
|
|
update_aux_servo_function();
|
|
|
|
// enable all channels that are set to a valid function. This
|
|
// includes k_none servos, which allows those to get their initial
|
|
// trim value on startup
|
|
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
|
SRV_Channel &c = channels[i];
|
|
// see if it is a valid function
|
|
if ((uint8_t)c.function.get() < SRV_Channel::k_nr_aux_servo_functions) {
|
|
hal.rcout->enable_ch(c.ch_num);
|
|
}
|
|
|
|
/*
|
|
for channels which have been marked as digital output then the
|
|
MIN/MAX/TRIM values have no meaning for controlling output, as
|
|
the HAL handles the scaling. We still need to cope with places
|
|
in the code that may try to set a PWM value however, so to
|
|
ensure consistency we force the MIN/MAX/TRIM to be consistent
|
|
across all digital channels. We use a MIN/MAX of 1000/2000, and
|
|
set TRIM to either 1000 or 1500 depending on whether the channel
|
|
is reversible
|
|
*/
|
|
if (digital_mask & (1U<<i)) {
|
|
c.servo_min.set(1000);
|
|
c.servo_max.set(2000);
|
|
if (reversible_mask & (1U<<i)) {
|
|
c.servo_trim.set(1500);
|
|
} else {
|
|
c.servo_trim.set(1000);
|
|
}
|
|
}
|
|
}
|
|
|
|
#if HAL_SUPPORT_RCOUT_SERIAL
|
|
blheli_ptr->update();
|
|
#endif
|
|
}
|
|
|
|
/// enable output channels using a channel mask
|
|
void SRV_Channels::enable_by_mask(uint16_t mask)
|
|
{
|
|
for (uint8_t i = 0; i < 16; i++) {
|
|
if (mask & (1U<<i)) {
|
|
hal.rcout->enable_ch(i);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
set radio_out for all channels matching the given function type
|
|
*/
|
|
void SRV_Channels::set_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t value)
|
|
{
|
|
if (!function_assigned(function)) {
|
|
return;
|
|
}
|
|
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
|
if (channels[i].function.get() == function) {
|
|
channels[i].set_output_pwm(value);
|
|
channels[i].output_ch();
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
set radio_out for all channels matching the given function type
|
|
trim the output assuming a 1500 center on the given value
|
|
reverses pwm output based on channel reversed property
|
|
*/
|
|
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;
|
|
if (channels[i].get_reversed()) {
|
|
value2 = 1500 - value + channels[i].get_trim();
|
|
} else {
|
|
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 current output for all channels matching
|
|
the given function type
|
|
*/
|
|
void
|
|
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::Aux_servo_function_t function)
|
|
{
|
|
if (!function_assigned(function)) {
|
|
return;
|
|
}
|
|
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
|
if (channels[i].function.get() == function) {
|
|
channels[i].servo_trim.set_and_save_ifchanged(channels[i].output_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 *c = rc().channel(channels[i].ch_num);
|
|
if (c == nullptr) {
|
|
continue;
|
|
}
|
|
channels[i].set_output_pwm(c->get_radio_in());
|
|
if (do_input_output) {
|
|
channels[i].output_ch();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
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 *c = rc().channel(channels[i].ch_num);
|
|
if (c == nullptr) {
|
|
continue;
|
|
}
|
|
channels[i].set_output_pwm(c->get_radio_in());
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
/*
|
|
setup failsafe value for an auxiliary function type to a Limit
|
|
*/
|
|
void
|
|
SRV_Channels::set_failsafe_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t pwm)
|
|
{
|
|
if (!function_assigned(function)) {
|
|
return;
|
|
}
|
|
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
|
const SRV_Channel &c = channels[i];
|
|
if (c.function.get() == function) {
|
|
hal.rcout->set_failsafe_pwm(1U<<c.ch_num, pwm);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
setup failsafe value for an auxiliary function type to a Limit
|
|
*/
|
|
void
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::Limit limit)
|
|
{
|
|
if (!function_assigned(function)) {
|
|
return;
|
|
}
|
|
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
|
const SRV_Channel &c = channels[i];
|
|
if (c.function.get() == function) {
|
|
uint16_t pwm = c.get_limit_pwm(limit);
|
|
hal.rcout->set_failsafe_pwm(1U<<c.ch_num, pwm);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
setup safety value for an auxiliary function type to a Limit
|
|
*/
|
|
void
|
|
SRV_Channels::set_safety_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::Limit limit)
|
|
{
|
|
if (!function_assigned(function)) {
|
|
return;
|
|
}
|
|
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
|
const SRV_Channel &c = channels[i];
|
|
if (c.function.get() == function) {
|
|
uint16_t pwm = c.get_limit_pwm(limit);
|
|
hal.rcout->set_safety_pwm(1U<<c.ch_num, pwm);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
set radio output value for an auxiliary function type to a Limit
|
|
*/
|
|
void
|
|
SRV_Channels::set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::Limit limit)
|
|
{
|
|
if (!function_assigned(function)) {
|
|
return;
|
|
}
|
|
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
|
SRV_Channel &c = channels[i];
|
|
if (c.function.get() == function) {
|
|
uint16_t pwm = c.get_limit_pwm(limit);
|
|
c.set_output_pwm(pwm);
|
|
if (c.function.get() == SRV_Channel::k_manual) {
|
|
RC_Channel *cin = rc().channel(c.ch_num);
|
|
if (cin != nullptr) {
|
|
// in order for output_ch() to work for k_manual we
|
|
// also have to override radio_in
|
|
cin->set_radio_in(pwm);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
return true if a particular function is assigned to at least one RC channel
|
|
*/
|
|
bool
|
|
SRV_Channels::function_assigned(SRV_Channel::Aux_servo_function_t function)
|
|
{
|
|
return function_mask.get(uint16_t(function));
|
|
}
|
|
|
|
/*
|
|
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 &c = channels[i];
|
|
if (c.function.get() == function) {
|
|
float v2 = c.get_reversed()? (1-v) : v;
|
|
uint16_t pwm = c.servo_min + v2 * (c.servo_max - c.servo_min);
|
|
c.set_output_pwm(pwm);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
set the default channel an auxiliary output function should be on
|
|
*/
|
|
bool SRV_Channels::set_aux_channel_default(SRV_Channel::Aux_servo_function_t function, uint8_t channel)
|
|
{
|
|
if (!initialised) {
|
|
update_aux_servo_function();
|
|
}
|
|
if (function_assigned(function)) {
|
|
// already assigned
|
|
return true;
|
|
}
|
|
if (channels[channel].function != SRV_Channel::k_none) {
|
|
if (channels[channel].function == function) {
|
|
return true;
|
|
}
|
|
hal.console->printf("Channel %u already assigned function %u\n",
|
|
(unsigned)(channel + 1),
|
|
(unsigned)channels[channel].function);
|
|
return false;
|
|
}
|
|
channels[channel].type_setup = false;
|
|
channels[channel].function.set(function);
|
|
channels[channel].aux_servo_function_setup();
|
|
function_mask.set((uint8_t)function);
|
|
functions[function].channel_mask |= 1U<<channel;
|
|
return true;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
/*
|
|
get a pointer to first auxillary channel for a channel function
|
|
*/
|
|
SRV_Channel *SRV_Channels::get_channel_for(SRV_Channel::Aux_servo_function_t function, int8_t default_chan)
|
|
{
|
|
uint8_t chan;
|
|
if (default_chan >= 0) {
|
|
set_aux_channel_default(function, default_chan);
|
|
}
|
|
if (!find_channel(function, chan)) {
|
|
return nullptr;
|
|
}
|
|
return &channels[chan];
|
|
}
|
|
|
|
void SRV_Channels::set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value)
|
|
{
|
|
if (function < SRV_Channel::k_nr_aux_servo_functions) {
|
|
functions[function].output_scaled = value;
|
|
SRV_Channel::have_pwm_mask &= ~functions[function].channel_mask;
|
|
}
|
|
}
|
|
|
|
int16_t SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t function)
|
|
{
|
|
if (function < SRV_Channel::k_nr_aux_servo_functions) {
|
|
return functions[function].output_scaled;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
get mask of output channels for a function
|
|
*/
|
|
uint16_t SRV_Channels::get_output_channel_mask(SRV_Channel::Aux_servo_function_t function)
|
|
{
|
|
if (!initialised) {
|
|
update_aux_servo_function();
|
|
}
|
|
if (function < SRV_Channel::k_nr_aux_servo_functions) {
|
|
return functions[function].channel_mask;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
// set the trim for a function channel to given pwm
|
|
void SRV_Channels::set_trim_to_pwm_for(SRV_Channel::Aux_servo_function_t function, int16_t pwm)
|
|
{
|
|
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
|
if (channels[i].function == function) {
|
|
channels[i].servo_trim.set(pwm);
|
|
}
|
|
}
|
|
}
|
|
|
|
// set the trim for a function channel to min output
|
|
void SRV_Channels::set_trim_to_min_for(SRV_Channel::Aux_servo_function_t function)
|
|
{
|
|
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
|
if (channels[i].function == function) {
|
|
channels[i].servo_trim.set(channels[i].get_reversed()?channels[i].servo_max:channels[i].servo_min);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
set the default function for a channel
|
|
*/
|
|
void SRV_Channels::set_default_function(uint8_t chan, SRV_Channel::Aux_servo_function_t function)
|
|
{
|
|
if (chan < NUM_SERVO_CHANNELS) {
|
|
int8_t old = channels[chan].function;
|
|
channels[chan].function.set_default((uint8_t)function);
|
|
if (old != channels[chan].function && channels[chan].function == function) {
|
|
function_mask.set((uint8_t)function);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SRV_Channels::set_esc_scaling_for(SRV_Channel::Aux_servo_function_t function)
|
|
{
|
|
uint8_t chan;
|
|
if (find_channel(function, chan)) {
|
|
hal.rcout->set_esc_scaling(channels[chan].get_output_min(), channels[chan].get_output_max());
|
|
}
|
|
}
|
|
|
|
/*
|
|
auto-adjust channel trim from an integrator value. Positive v means
|
|
adjust trim up. Negative means decrease
|
|
*/
|
|
void SRV_Channels::adjust_trim(SRV_Channel::Aux_servo_function_t function, float v)
|
|
{
|
|
if (is_zero(v)) {
|
|
return;
|
|
}
|
|
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
|
SRV_Channel &c = channels[i];
|
|
if (function != (SRV_Channel::Aux_servo_function_t)(c.function.get())) {
|
|
continue;
|
|
}
|
|
float change = c.reversed?-v:v;
|
|
uint16_t new_trim = c.servo_trim;
|
|
float trim_scaled = float(c.servo_trim - c.servo_min) / (c.servo_max - c.servo_min);
|
|
if (change > 0 && trim_scaled < 0.6f) {
|
|
new_trim++;
|
|
} else if (change < 0 && trim_scaled > 0.4f) {
|
|
new_trim--;
|
|
} else {
|
|
return;
|
|
}
|
|
c.servo_trim.set(new_trim);
|
|
|
|
trimmed_mask |= 1U<<i;
|
|
}
|
|
}
|
|
|
|
// get pwm output for the first channel of the given function type.
|
|
bool SRV_Channels::get_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t &value)
|
|
{
|
|
uint8_t chan;
|
|
if (!find_channel(function, chan)) {
|
|
return false;
|
|
}
|
|
channels[chan].calc_pwm(functions[function].output_scaled);
|
|
value = channels[chan].output_pwm;
|
|
return true;
|
|
}
|
|
|
|
// set output pwm to trim for the given function
|
|
void SRV_Channels::set_output_to_trim(SRV_Channel::Aux_servo_function_t function)
|
|
{
|
|
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
|
if (channels[i].function == function) {
|
|
channels[i].set_output_pwm(channels[i].servo_trim);
|
|
}
|
|
}
|
|
}
|
|
|
|
// set output pwm to for first matching channel
|
|
void SRV_Channels::set_output_pwm_first(SRV_Channel::Aux_servo_function_t function, uint16_t pwm)
|
|
{
|
|
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
|
if (channels[i].function == function) {
|
|
channels[i].set_output_pwm(pwm);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
get the normalised output for a channel function from the pwm value
|
|
of the first matching channel
|
|
*/
|
|
float SRV_Channels::get_output_norm(SRV_Channel::Aux_servo_function_t function)
|
|
{
|
|
uint8_t chan;
|
|
if (!find_channel(function, chan)) {
|
|
return 0;
|
|
}
|
|
channels[chan].calc_pwm(functions[function].output_scaled);
|
|
return channels[chan].get_output_norm();
|
|
}
|
|
|
|
/*
|
|
limit slew rate for an output function to given rate in percent per
|
|
second. This assumes output has not yet done to the hal
|
|
*/
|
|
void SRV_Channels::limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, float dt)
|
|
{
|
|
if (slew_rate <= 0) {
|
|
// nothing to do
|
|
return;
|
|
}
|
|
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
|
SRV_Channel &c = channels[i];
|
|
if (c.function == function) {
|
|
c.calc_pwm(functions[function].output_scaled);
|
|
uint16_t last_pwm = hal.rcout->read_last_sent(c.ch_num);
|
|
if (last_pwm == c.output_pwm) {
|
|
continue;
|
|
}
|
|
uint16_t max_change = (c.get_output_max() - c.get_output_min()) * slew_rate * dt * 0.01f;
|
|
if (max_change == 0 || dt > 1) {
|
|
// always allow some change. If dt > 1 then assume we
|
|
// are just starting out, and only allow a small
|
|
// change for this loop
|
|
max_change = 1;
|
|
}
|
|
c.output_pwm = constrain_int16(c.output_pwm, last_pwm-max_change, last_pwm+max_change);
|
|
}
|
|
}
|
|
}
|
|
|
|
// call set_angle() on matching channels
|
|
void SRV_Channels::set_angle(SRV_Channel::Aux_servo_function_t function, uint16_t angle)
|
|
{
|
|
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
|
if (channels[i].function == function) {
|
|
channels[i].set_angle(angle);
|
|
}
|
|
}
|
|
}
|
|
|
|
// call set_range() on matching channels
|
|
void SRV_Channels::set_range(SRV_Channel::Aux_servo_function_t function, uint16_t range)
|
|
{
|
|
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
|
if (channels[i].function == function) {
|
|
channels[i].set_range(range);
|
|
}
|
|
}
|
|
}
|
|
|
|
// set MIN parameter for a function
|
|
void SRV_Channels::set_output_min_max(SRV_Channel::Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm)
|
|
{
|
|
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
|
if (channels[i].function == function) {
|
|
channels[i].set_output_min(min_pwm);
|
|
channels[i].set_output_max(max_pwm);
|
|
}
|
|
}
|
|
}
|
|
|
|
// constrain to output min/max for function
|
|
void SRV_Channels::constrain_pwm(SRV_Channel::Aux_servo_function_t function)
|
|
{
|
|
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
|
SRV_Channel &c = channels[i];
|
|
if (c.function == function) {
|
|
c.output_pwm = constrain_int16(c.output_pwm, c.servo_min, c.servo_max);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
upgrade RC* parameters into SERVO* parameters. This does the following:
|
|
|
|
- copies MIN/MAX/TRIM values from old RC parameters into new RC* parameters and SERVO* parameters.
|
|
- copies RCn_FUNCTION to SERVOn_FUNCTION
|
|
- maps old RCn_REV to SERVOn_REVERSE and RCn_REVERSE
|
|
|
|
aux_channel_mask is a bitmask of which channels were RC_Channel_aux channels
|
|
|
|
Note that this code is highly dependent on the parameter indexing of
|
|
the old RC_Channel and RC_Channel_aux objects.
|
|
|
|
If rcmap is passed in then the vehicle code also wants functions for
|
|
the first 4 output channels to be remapped
|
|
|
|
We return true if an upgrade has been done. This allows the caller
|
|
to make any vehicle specific upgrades that may be needed
|
|
*/
|
|
bool SRV_Channels::upgrade_parameters(const uint8_t rc_keys[14], uint16_t aux_channel_mask, RCMapper *rcmap)
|
|
{
|
|
// use SERVO16_FUNCTION as a marker to say that we have run the upgrade already
|
|
if (channels[15].function.configured_in_storage()) {
|
|
// upgrade already done
|
|
return false;
|
|
}
|
|
|
|
// old system had 14 RC channels
|
|
for (uint8_t i=0; i<14; i++) {
|
|
uint8_t k = rc_keys[i];
|
|
if (k == 0) {
|
|
// missing parameter. Some vehicle types didn't have all parameters
|
|
continue;
|
|
}
|
|
SRV_Channel &srv_chan = channels[i];
|
|
RC_Channel *rc_chan = rc().channel(i);
|
|
enum {
|
|
FLAG_NONE=0,
|
|
FLAG_IS_REVERSE=1,
|
|
FLAG_AUX_ONLY=2
|
|
};
|
|
const struct mapping {
|
|
uint8_t old_index;
|
|
AP_Param *new_srv_param;
|
|
AP_Param *new_rc_param;
|
|
enum ap_var_type type;
|
|
uint8_t flags;
|
|
} mapping[] = {
|
|
{ 0, &srv_chan.servo_min, &rc_chan->radio_min, AP_PARAM_INT16, FLAG_NONE },
|
|
{ 1, &srv_chan.servo_trim, &rc_chan->radio_trim, AP_PARAM_INT16, FLAG_NONE },
|
|
{ 2, &srv_chan.servo_max, &rc_chan->radio_max, AP_PARAM_INT16, FLAG_NONE },
|
|
{ 3, &srv_chan.reversed, &rc_chan->reversed, AP_PARAM_INT8, FLAG_IS_REVERSE },
|
|
{ 1, &srv_chan.function, nullptr, AP_PARAM_INT8, FLAG_AUX_ONLY },
|
|
};
|
|
bool is_aux = aux_channel_mask & (1U<<i);
|
|
|
|
for (uint8_t j=0; j<ARRAY_SIZE(mapping); j++) {
|
|
const struct mapping &m = mapping[j];
|
|
AP_Param::ConversionInfo info;
|
|
AP_Int8 v8;
|
|
AP_Int16 v16;
|
|
AP_Param *v = m.type == AP_PARAM_INT16?(AP_Param*)&v16:(AP_Param*)&v8;
|
|
bool aux_only = (m.flags & FLAG_AUX_ONLY)!=0;
|
|
if (!is_aux && aux_only) {
|
|
continue;
|
|
}
|
|
info.old_key = k;
|
|
info.type = m.type;
|
|
info.new_name = nullptr;
|
|
|
|
// if this was an aux channel we need to shift by 6 bits, but not for RCn_FUNCTION
|
|
info.old_group_element = (is_aux && !aux_only)?(m.old_index<<6):m.old_index;
|
|
|
|
if (!AP_Param::find_old_parameter(&info, v)) {
|
|
// the parameter wasn't set in the old eeprom
|
|
continue;
|
|
}
|
|
|
|
if (m.flags & FLAG_IS_REVERSE) {
|
|
// special mapping from RCn_REV to RCn_REVERSED
|
|
v8.set(v8.get() == -1?1:0);
|
|
}
|
|
|
|
if (!m.new_srv_param->configured_in_storage()) {
|
|
// not configured yet in new eeprom
|
|
if (m.type == AP_PARAM_INT16) {
|
|
((AP_Int16 *)m.new_srv_param)->set_and_save_ifchanged(v16.get());
|
|
} else {
|
|
((AP_Int8 *)m.new_srv_param)->set_and_save_ifchanged(v8.get());
|
|
}
|
|
}
|
|
if (m.new_rc_param && !m.new_rc_param->configured_in_storage()) {
|
|
// not configured yet in new eeprom
|
|
if (m.type == AP_PARAM_INT16) {
|
|
((AP_Int16 *)m.new_rc_param)->set_and_save_ifchanged(v16.get());
|
|
} else {
|
|
((AP_Int8 *)m.new_rc_param)->set_and_save_ifchanged(v8.get());
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (rcmap != nullptr) {
|
|
// we need to make the output functions from the rcmapped inputs
|
|
const int8_t func_map[4] = { channels[0].function.get(),
|
|
channels[1].function.get(),
|
|
channels[2].function.get(),
|
|
channels[3].function.get() };
|
|
const uint8_t map[4] = { rcmap->roll(), rcmap->pitch(), rcmap->throttle(), rcmap->yaw() };
|
|
for (uint8_t i=0; i<4; i++) {
|
|
uint8_t m = uint8_t(map[i]-1);
|
|
if (m != i && m < 4) {
|
|
channels[m].function.set_and_save_ifchanged(func_map[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
// mark the upgrade as having been done
|
|
channels[15].function.set_and_save(channels[15].function.get());
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
Upgrade servo MIN/MAX/TRIM/REVERSE parameters for a single AP_Motors
|
|
RC_Channel servo from previous firmwares, setting the equivalent
|
|
parameter in the new SRV_Channels object
|
|
*/
|
|
void SRV_Channels::upgrade_motors_servo(uint8_t ap_motors_key, uint8_t ap_motors_idx, uint8_t new_channel)
|
|
{
|
|
SRV_Channel &srv_chan = channels[new_channel];
|
|
enum {
|
|
FLAG_NONE=0,
|
|
FLAG_IS_REVERSE=1
|
|
};
|
|
const struct mapping {
|
|
uint8_t old_index;
|
|
AP_Param *new_srv_param;
|
|
enum ap_var_type type;
|
|
uint8_t flags;
|
|
} mapping[] = {
|
|
{ 0, &srv_chan.servo_min, AP_PARAM_INT16, FLAG_NONE },
|
|
{ 1, &srv_chan.servo_trim, AP_PARAM_INT16, FLAG_NONE },
|
|
{ 2, &srv_chan.servo_max, AP_PARAM_INT16, FLAG_NONE },
|
|
{ 3, &srv_chan.reversed, AP_PARAM_INT8, FLAG_IS_REVERSE },
|
|
};
|
|
|
|
for (uint8_t j=0; j<ARRAY_SIZE(mapping); j++) {
|
|
const struct mapping &m = mapping[j];
|
|
AP_Param::ConversionInfo info;
|
|
AP_Int8 v8;
|
|
AP_Int16 v16;
|
|
AP_Param *v = m.type == AP_PARAM_INT16?(AP_Param*)&v16:(AP_Param*)&v8;
|
|
|
|
info.old_key = ap_motors_key;
|
|
info.type = m.type;
|
|
info.new_name = nullptr;
|
|
info.old_group_element = ap_motors_idx | (m.old_index<<6);
|
|
|
|
if (!AP_Param::find_old_parameter(&info, v)) {
|
|
// the parameter wasn't set in the old eeprom
|
|
continue;
|
|
}
|
|
|
|
if (m.flags & FLAG_IS_REVERSE) {
|
|
// special mapping from RCn_REV to RCn_REVERSED
|
|
v8.set(v8.get() == -1?1:0);
|
|
}
|
|
|
|
// we save even if there is already a value in the new eeprom,
|
|
// as that may come from the equivalent RC channel, not the
|
|
// old motor servo channel
|
|
if (m.type == AP_PARAM_INT16) {
|
|
((AP_Int16 *)m.new_srv_param)->set_and_save_ifchanged(v16.get());
|
|
} else {
|
|
((AP_Int8 *)m.new_srv_param)->set_and_save_ifchanged(v8.get());
|
|
}
|
|
}
|
|
}
|
|
|
|
// set RC output frequency on a function output
|
|
void SRV_Channels::set_rc_frequency(SRV_Channel::Aux_servo_function_t function, uint16_t frequency_hz)
|
|
{
|
|
uint16_t mask = 0;
|
|
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
|
|
SRV_Channel &c = channels[i];
|
|
if (c.function == function) {
|
|
mask |= (1U<<c.ch_num);
|
|
}
|
|
}
|
|
if (mask != 0) {
|
|
hal.rcout->set_freq(mask, frequency_hz);
|
|
}
|
|
}
|