From fd081a8cc6c43313086be18969b0f1213e8a9513 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Jan 2017 12:02:32 +1100 Subject: [PATCH] SRV_Channel: split SRV_Channel into separate directory --- libraries/SRV_Channel/SRV_Channel.cpp | 319 ++++++++++++ libraries/SRV_Channel/SRV_Channel.h | 387 ++++++++++++++ libraries/SRV_Channel/SRV_Channel_aux.cpp | 581 ++++++++++++++++++++++ 3 files changed, 1287 insertions(+) create mode 100644 libraries/SRV_Channel/SRV_Channel.cpp create mode 100644 libraries/SRV_Channel/SRV_Channel.h create mode 100644 libraries/SRV_Channel/SRV_Channel_aux.cpp diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp new file mode 100644 index 0000000000..0cf796c419 --- /dev/null +++ b/libraries/SRV_Channel/SRV_Channel.cpp @@ -0,0 +1,319 @@ +/* + 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 . + */ +/* + SRV_Channel.cpp - object to separate input and output channel + ranges, trim and reversal + */ + +#include +#include +#include +#include "SRV_Channel.h" + +extern const AP_HAL::HAL& hal; + +SRV_Channel *SRV_Channels::channels; +bool SRV_Channels::disabled_passthrough; +bool SRV_Channels::initialised; +Bitmask SRV_Channels::function_mask{SRV_Channel::k_nr_aux_servo_functions}; +SRV_Channels::srv_function SRV_Channels::functions[SRV_Channel::k_nr_aux_servo_functions]; +SRV_Channel::servo_mask_t SRV_Channel::have_pwm_mask; + +const AP_Param::GroupInfo SRV_Channel::var_info[] = { + // @Param: MIN + // @DisplayName: Minimum PWM + // @Description: minimum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. + // @Units: pwm + // @Range: 800 2200 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("MIN", 1, SRV_Channel, servo_min, 1100), + + // @Param: MAX + // @DisplayName: Maximum PWM + // @Description: maximum PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. + // @Units: pwm + // @Range: 800 2200 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("MAX", 2, SRV_Channel, servo_max, 1900), + + // @Param: TRIM + // @DisplayName: Trim PWM + // @Description: Trim PWM pulse width. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit. + // @Units: pwm + // @Range: 800 2200 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("TRIM", 3, SRV_Channel, servo_trim, 1500), + + // @Param: REVERSED + // @DisplayName: Servo reverse + // @Description: Reverse servo operation. Set to 0 for normal operation. Set to 1 to reverse this channel. + // @Values: 0:Normal,1:Reversed + // @User: Standard + AP_GROUPINFO("REVERSED", 4, SRV_Channel, reversed, 0), + + // @Param: FUNCTION + // @DisplayName: Servo output function + // @Description: Function assigned to this servo. Seeing this to Disabled(0) will setup this output for control by auto missions or MAVLink servo set commands. any other value will enable the corresponding function + // @Values: 0:Disabled,1:RCPassThru,2:Flap,3:Flap_auto,4:Aileron,6:mount_pan,7:mount_tilt,8:mount_roll,9:mount_open,10:camera_trigger,11:release,12:mount2_pan,13:mount2_tilt,14:mount2_roll,15:mount2_open,16:DifferentialSpoiler1,17:DifferentialSpoiler2,18:AileronWithInput,19:Elevator,20:ElevatorWithInput,21:Rudder,24:Flaperon1,25:Flaperon2,26:GroundSteering,27:Parachute,28:EPM,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,67:Ignition,68:Choke,69:Starter,70:Throttle + // @User: Standard + AP_GROUPINFO("FUNCTION", 5, SRV_Channel, function, 0), + + AP_GROUPEND +}; + + +const AP_Param::GroupInfo SRV_Channels::var_info[] = { + // @Group: 1_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[0], "1_", 1, SRV_Channels, SRV_Channel), + + // @Group: 2_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[1], "2_", 2, SRV_Channels, SRV_Channel), + + // @Group: 3_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[2], "3_", 3, SRV_Channels, SRV_Channel), + + // @Group: 4_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[3], "4_", 4, SRV_Channels, SRV_Channel), + + // @Group: 5_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[4], "5_", 5, SRV_Channels, SRV_Channel), + + // @Group: 6_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[5], "6_", 6, SRV_Channels, SRV_Channel), + + // @Group: 7_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[6], "7_", 7, SRV_Channels, SRV_Channel), + + // @Group: 8_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[7], "8_", 8, SRV_Channels, SRV_Channel), + + // @Group: 9_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[8], "9_", 9, SRV_Channels, SRV_Channel), + + // @Group: 10_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[9], "10_", 10, SRV_Channels, SRV_Channel), + + // @Group: 11_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[10], "11_", 11, SRV_Channels, SRV_Channel), + + // @Group: 12_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[11], "12_", 12, SRV_Channels, SRV_Channel), + + // @Group: 13_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[12], "13_", 13, SRV_Channels, SRV_Channel), + + // @Group: 14_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[13], "14_", 14, SRV_Channels, SRV_Channel), + + // @Group: 15_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[14], "15_", 15, SRV_Channels, SRV_Channel), + + // @Group: 16_ + // @Path: SRV_Channel.cpp + AP_SUBGROUPINFO(obj_channels[15], "16_", 16, SRV_Channels, SRV_Channel), + + // @Param: _AUTO_TRIM + // @DisplayName: Automatic servo trim + // @Description: This enables automatic servo trim in flight. Servos will be trimed in stabilized flight modes when the aircraft is close to level. Changes to servo trim will be saved every 10 seconds and will persist between flights. + // @Values: 0:Disable,1:Enable + // @User: Advanced + AP_GROUPINFO("_AUTO_TRIM", 17, SRV_Channels, auto_trim, 0), + + AP_GROUPEND +}; + +SRV_Channel::SRV_Channel(void) +{ + AP_Param::setup_object_defaults(this, var_info); + // start with all pwm at zero + have_pwm_mask = ~uint16_t(0); +} + + +/* + constructor + */ +SRV_Channels::SRV_Channels(void) +{ + channels = obj_channels; + + // set defaults from the parameter table + AP_Param::setup_object_defaults(this, var_info); + + // setup ch_num on channels + for (uint8_t i=0; i= high_out) { + scaled_value = high_out; + } + if (scaled_value < 0) { + scaled_value = 0; + } + if (reversed) { + scaled_value = high_out - scaled_value; + } + return servo_min + ((int32_t)scaled_value * (int32_t)(servo_max - servo_min)) / (int32_t)high_out; +} + +// convert a -angle_max..angle_max to a pwm +uint16_t SRV_Channel::pwm_from_angle(int16_t scaled_value) const +{ + if (reversed) { + scaled_value = -scaled_value; + } + if (scaled_value > 0) { + return servo_trim + ((int32_t)scaled_value * (int32_t)(servo_max - servo_trim)) / (int32_t)high_out; + } else { + return servo_trim - (-(int32_t)scaled_value * (int32_t)(servo_trim - servo_min)) / (int32_t)high_out; + } +} + +void SRV_Channel::calc_pwm(int16_t output_scaled) +{ + if (have_pwm_mask & (1U<set_failsafe_pwm(1U< mid) { + ret = (float)(output_pwm - mid) / (float)(servo_max - mid); + } else { + ret = 0; + } + if (get_reversed()) { + ret = -ret; + } + return ret; +} + +uint16_t SRV_Channel::get_limit_pwm(LimitValue limit) const +{ + switch (limit) { + case SRV_CHANNEL_LIMIT_TRIM: + return servo_trim; + case SRV_CHANNEL_LIMIT_MIN: + return servo_min; + case SRV_CHANNEL_LIMIT_MAX: + return servo_max; + case SRV_CHANNEL_LIMIT_ZERO_PWM: + default: + return 0; + } +} + +/* + run calc_pwm for all channels + */ +void SRV_Channels::calc_pwm(void) +{ + for (uint8_t i=0; i +#include +#include +#include + +#define NUM_SERVO_CHANNELS 16 + +class SRV_Channels; + +/* + class SRV_Channel. The class SRV_Channels contains an array of + SRV_Channel objects. This is done to fit within the AP_Param limit + of 64 parameters per object. +*/ +class SRV_Channel { +public: + friend class SRV_Channels; + // constructor + SRV_Channel(void); + + static const struct AP_Param::GroupInfo var_info[]; + + typedef enum + { + k_none = 0, ///< disabled + k_manual = 1, ///< manual, just pass-thru the RC in signal + k_flap = 2, ///< flap + k_flap_auto = 3, ///< flap automated + k_aileron = 4, ///< aileron + k_unused1 = 5, ///< unused function + k_mount_pan = 6, ///< mount yaw (pan) + k_mount_tilt = 7, ///< mount pitch (tilt) + k_mount_roll = 8, ///< mount roll + k_mount_open = 9, ///< mount open (deploy) / close (retract) + k_cam_trigger = 10, ///< camera trigger + k_egg_drop = 11, ///< egg drop + k_mount2_pan = 12, ///< mount2 yaw (pan) + k_mount2_tilt = 13, ///< mount2 pitch (tilt) + k_mount2_roll = 14, ///< mount2 roll + k_mount2_open = 15, ///< mount2 open (deploy) / close (retract) + k_dspoiler1 = 16, ///< differential spoiler 1 (left wing) + k_dspoiler2 = 17, ///< differential spoiler 2 (right wing) + k_aileron_with_input = 18, ///< aileron, with rc input + k_elevator = 19, ///< elevator + k_elevator_with_input = 20, ///< elevator, with rc input + k_rudder = 21, ///< secondary rudder channel + k_sprayer_pump = 22, ///< crop sprayer pump channel + k_sprayer_spinner = 23, ///< crop sprayer spinner channel + k_flaperon1 = 24, ///< flaperon, left wing + k_flaperon2 = 25, ///< flaperon, right wing + k_steering = 26, ///< ground steering, used to separate from rudder + k_parachute_release = 27, ///< parachute release + k_gripper = 28, ///< gripper + k_landing_gear_control = 29, ///< landing gear controller + k_engine_run_enable = 30, ///< engine kill switch, used for gas airplanes and helicopters + k_heli_rsc = 31, ///< helicopter RSC output + k_heli_tail_rsc = 32, ///< helicopter tail RSC output + k_motor1 = 33, ///< these allow remapping of copter motors + k_motor2 = 34, + k_motor3 = 35, + k_motor4 = 36, + k_motor5 = 37, + k_motor6 = 38, + k_motor7 = 39, + k_motor8 = 40, + k_motor_tilt = 41, ///< tiltrotor motor tilt control + k_rcin1 = 51, ///< these are for pass-thru from arbitrary rc inputs + k_rcin2 = 52, + k_rcin3 = 53, + k_rcin4 = 54, + k_rcin5 = 55, + k_rcin6 = 56, + k_rcin7 = 57, + k_rcin8 = 58, + k_rcin9 = 59, + k_rcin10 = 60, + k_rcin11 = 61, + k_rcin12 = 62, + k_rcin13 = 63, + k_rcin14 = 64, + k_rcin15 = 65, + k_rcin16 = 66, + k_ignition = 67, + k_choke = 68, + k_starter = 69, + k_throttle = 70, + k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) + } Aux_servo_function_t; + + // used to get min/max/trim limit value based on reverse + enum LimitValue { + SRV_CHANNEL_LIMIT_TRIM, + SRV_CHANNEL_LIMIT_MIN, + SRV_CHANNEL_LIMIT_MAX, + SRV_CHANNEL_LIMIT_ZERO_PWM + }; + + // a special scaled output value that is recognised as meaning no pwm output + static const int16_t ZERO_PWM = INT16_MIN; + + // set the output value as a pwm value + void set_output_pwm(uint16_t pwm); + + // get the output value as a pwm value + uint16_t get_output_pwm(void) const { return output_pwm; } + + // set angular range of scaled output + void set_angle(int16_t angle); + + // set range of scaled output. Low is always zero + void set_range(uint16_t high); + + // return true if the channel is reversed + bool get_reversed(void) const { + return reversed?true:false; + } + + // set MIN/MAX parameters + void set_output_min(uint16_t pwm) { + servo_min.set(pwm); + } + void set_output_max(uint16_t pwm) { + servo_max.set(pwm); + } + + // get MIN/MAX/TRIM parameters + uint16_t get_output_min(void) const { + return servo_min; + } + uint16_t get_output_max(void) const { + return servo_max; + } + uint16_t get_trim(void) const { + return servo_trim; + } + +private: + AP_Int16 servo_min; + AP_Int16 servo_max; + AP_Int16 servo_trim; + // reversal, following convention that 1 means reversed, 0 means normal + AP_Int8 reversed; + AP_Int8 function; + + // a pending output value as PWM + uint16_t output_pwm; + + // true for angle output type + bool type_angle:1; + + // set_range() or set_angle() has been called + bool type_setup:1; + + // the hal channel number + uint8_t ch_num; + + // high point of angle or range output + uint16_t high_out; + + // convert a 0..range_max to a pwm + uint16_t pwm_from_range(int16_t scaled_value) const; + + // convert a -angle_max..angle_max to a pwm + uint16_t pwm_from_angle(int16_t scaled_value) const; + + // convert a scaled output to a pwm value + void calc_pwm(int16_t output_scaled); + + // output value based on function + void output_ch(void); + + // setup output type and range based on function + void aux_servo_function_setup(void); + + // return PWM for a given limit value + uint16_t get_limit_pwm(LimitValue limit) const; + + // get normalised output from -1 to 1 + float get_output_norm(void); + + // a bitmask type wide enough for NUM_SERVO_CHANNELS + typedef uint16_t servo_mask_t; + + // mask of channels where we have a output_pwm value. Cleared when a + // scaled value is written. + static servo_mask_t have_pwm_mask; +}; + +/* + class SRV_Channels +*/ +class SRV_Channels { +public: + // constructor + SRV_Channels(void); + + static const struct AP_Param::GroupInfo var_info[]; + + // set the default function for a channel + static void set_default_function(uint8_t chan, SRV_Channel::Aux_servo_function_t function); + + // set output value for a function channel as a pwm value + static void set_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t value); + + // set output value for a function channel as a pwm value on the first matching channel + static void set_output_pwm_first(SRV_Channel::Aux_servo_function_t function, uint16_t value); + + // set output value for a function channel as a scaled value. This + // calls calc_pwm() to also set the pwm value + static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, int16_t value); + + // get scaled output for the given function type. + static int16_t get_output_scaled(SRV_Channel::Aux_servo_function_t function); + + // get pwm output for the first channel of the given function type. + static bool get_output_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t &value); + + // get normalised output (-1 to 1 for angle, 0 to 1 for range). Value is taken from pwm value + // return zero on error. + static float get_output_norm(SRV_Channel::Aux_servo_function_t function); + + // limit slew rate to given limit in percent per second + static void limit_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate); + + // call output_ch() on all channels + static void output_ch_all(void); + + // take current radio_out for first 4 channels and remap using + // servo ranges if enabled + void remap_servo_output(void); + + // setup output ESC scaling based on a channels MIN/MAX + void set_esc_scaling_for(SRV_Channel::Aux_servo_function_t function); + + // return true when auto_trim enabled + bool auto_trim_enabled(void) const { return auto_trim; } + + // adjust trim of a channel by a small increment + void adjust_trim(SRV_Channel::Aux_servo_function_t function, float v); + + // save trims + void save_trim(void); + + // setup for a reversible k_throttle (from -100 to 100) + void set_reversible_throttle(void) { + flags.k_throttle_reversible = true; + } + + // set all outputs to the TRIM value + static void output_trim_all(void); + + // setup IO failsafe for all channels to trim + static void setup_failsafe_trim_all(void); + + // set output for all channels matching the given function type, allow radio_trim to center servo + static void set_output_pwm_trimmed(SRV_Channel::Aux_servo_function_t function, int16_t value); + + // set and save the trim for a function channel to radio_in on matching input channel + static void set_trim_to_radio_in_for(SRV_Channel::Aux_servo_function_t function); + + // set the trim for a function channel to min of the channel + static void set_trim_to_min_for(SRV_Channel::Aux_servo_function_t function); + + // set the trim for a function channel to given pwm + static void set_trim_to_pwm_for(SRV_Channel::Aux_servo_function_t function, int16_t pwm); + + // set output to min value + static void set_output_to_min(SRV_Channel::Aux_servo_function_t function); + + // set output to max value + static void set_output_to_max(SRV_Channel::Aux_servo_function_t function); + + // set output to trim value + static void set_output_to_trim(SRV_Channel::Aux_servo_function_t function); + + // copy radio_in to radio_out + static void copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output=false); + + // setup failsafe for an auxiliary channel function, by pwm + static void set_failsafe_pwm(SRV_Channel::SRV_Channel::Aux_servo_function_t function, uint16_t pwm); + + // setup failsafe for an auxiliary channel function + static void set_failsafe_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit); + + // setup safety for an auxiliary channel function (used when disarmed) + static void set_safety_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit); + + // set servo to a LimitValue + static void set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::LimitValue limit); + + // return true if a function is assigned to a channel + static bool function_assigned(SRV_Channel::Aux_servo_function_t function); + + // set a servo_out value, and angle range, then calc_pwm + static void move_servo(SRV_Channel::Aux_servo_function_t function, + int16_t value, int16_t angle_min, int16_t angle_max); + + // assign and enable auxiliary channels + static void enable_aux_servos(void); + + // prevent a channel from being used for auxiliary functions + static void disable_aux_channel(uint8_t channel); + + // return the current function for a channel + static SRV_Channel::Aux_servo_function_t channel_function(uint8_t channel); + + // refresh aux servo to function mapping + static void update_aux_servo_function(void); + + // set default channel for an auxiliary function + static bool set_aux_channel_default(SRV_Channel::Aux_servo_function_t function, uint8_t channel); + + // find first channel that a function is assigned to + static bool find_channel(SRV_Channel::Aux_servo_function_t function, uint8_t &chan); + + // find first channel that a function is assigned to, returning SRV_Channel object + static SRV_Channel *get_channel_for(SRV_Channel::Aux_servo_function_t function, int8_t default_chan=-1); + + // call set_angle() on matching channels + static void set_angle(SRV_Channel::Aux_servo_function_t function, uint16_t angle); + + // call set_range() on matching channels + static void set_range(SRV_Channel::Aux_servo_function_t function, uint16_t range); + + // control pass-thru of channels + void disable_passthrough(bool disable) { + disabled_passthrough = disable; + } + + static bool passthrough_disabled(void) { + return disabled_passthrough; + } + + // constrain to output min/max for function + static void constrain_pwm(SRV_Channel::Aux_servo_function_t function); + + // calculate PWM for all channels + static void calc_pwm(void); + + static SRV_Channel *srv_channel(uint8_t i) { + return i. + */ +/* + SRV_Channel_aux.cpp - handling of servo auxillary functions + */ +#include "SRV_Channel.h" + +#include +#include +#include + +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; + case k_motor1 ... k_motor8: + // handled by AP_Motors::rc_write() + return; + } + 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(); + } + } + } + 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: + set_range(1000); + break; + case k_aileron_with_input: + case k_elevator_with_input: + case k_aileron: + case k_elevator: + case k_dspoiler1: + case k_dspoiler2: + case k_rudder: + case k_steering: + case k_flaperon1: + case k_flaperon2: + set_angle(4500); + break; + case k_throttle: + // 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) +{ + 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++) { + channels[i].aux_servo_function_setup(); + function_mask.set((uint8_t)channels[i].function.get()); + functions[channels[i].function.get()].channel_mask |= 1U<enable_ch(channels[i].ch_num); + } + } +} + +/* + 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 + */ +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 +SRV_Channels::set_trim_to_radio_in_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) { + RC_Channel *rc = RC_Channels::rc_channel(channels[i].ch_num); + if (rc && rc->get_radio_in() != 0) { + rc->set_radio_trim(rc->get_radio_in()); + rc->save_radio_trim(); + } + } + } +} + +/* + 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(); + } + } + } +} + +/* + setup failsafe value for an auxiliary function type to a LimitValue + */ +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 &ch = channels[i]; + if (ch.function.get() == function) { + hal.rcout->set_failsafe_pwm(1U<set_failsafe_pwm(1U<set_safety_pwm(1U<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); + for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + SRV_Channel &ch = channels[i]; + if (ch.function.get() == function) { + uint16_t pwm = ch.servo_min + v * (ch.servo_max - ch.servo_min); + ch.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 %u\n", + (unsigned)channel, + (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); + 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= 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; +} + + +// 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; iset_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 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<