mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
1cbad0c86e
this also removes the old parameter conversion code used for when we first added the SERVO parameters. This was needed for conversion from Copter 3.4 and Plane 3.7
561 lines
21 KiB
C++
561 lines
21 KiB
C++
/*
|
|
control of servo output ranges, trim and servo reversal. This can
|
|
optionally be used to provide separation of input and output channel
|
|
ranges so that RCn_MIN, RCn_MAX, RCn_TRIM and RCn_REV only apply to
|
|
the input side of RC_Channel
|
|
|
|
It works by running servo output calculations as normal, then
|
|
re-mapping the output according to the servo MIN/MAX/TRIM/REV from
|
|
this object
|
|
|
|
Only 4 channels of ranges are defined as those match the input
|
|
channels for R/C sticks
|
|
*/
|
|
#pragma once
|
|
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <AP_RCMapper/AP_RCMapper.h>
|
|
#include <AP_Common/Bitmask.h>
|
|
#include <AP_Volz_Protocol/AP_Volz_Protocol.h>
|
|
#include <AP_RobotisServo/AP_RobotisServo.h>
|
|
#include <AP_SBusOut/AP_SBusOut.h>
|
|
#include <AP_BLHeli/AP_BLHeli.h>
|
|
|
|
#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_dspoilerLeft1 = 16, ///< differential spoiler 1 (left wing)
|
|
k_dspoilerRight1 = 17, ///< differential spoiler 1 (right wing)
|
|
k_aileron_with_input = 18, ///< aileron, with rc input, deprecated
|
|
k_elevator = 19, ///< elevator
|
|
k_elevator_with_input = 20, ///< elevator, with rc input, deprecated
|
|
k_rudder = 21, ///< secondary rudder channel
|
|
k_sprayer_pump = 22, ///< crop sprayer pump channel
|
|
k_sprayer_spinner = 23, ///< crop sprayer spinner channel
|
|
k_flaperon_left = 24, ///< flaperon, left wing
|
|
k_flaperon_right = 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_tracker_yaw = 71, ///< antennatracker yaw
|
|
k_tracker_pitch = 72, ///< antennatracker pitch
|
|
k_throttleLeft = 73,
|
|
k_throttleRight = 74,
|
|
k_tiltMotorLeft = 75, ///< vectored thrust, left tilt
|
|
k_tiltMotorRight = 76, ///< vectored thrust, right tilt
|
|
k_elevon_left = 77,
|
|
k_elevon_right = 78,
|
|
k_vtail_left = 79,
|
|
k_vtail_right = 80,
|
|
k_boost_throttle = 81, ///< vertical booster throttle
|
|
k_motor9 = 82,
|
|
k_motor10 = 83,
|
|
k_motor11 = 84,
|
|
k_motor12 = 85,
|
|
k_dspoilerLeft2 = 86, ///< differential spoiler 2 (left wing)
|
|
k_dspoilerRight2 = 87, ///< differential spoiler 2 (right wing)
|
|
k_winch = 88,
|
|
k_mainsail_sheet = 89, ///< Main Sail control via sheet
|
|
k_cam_iso = 90,
|
|
k_cam_aperture = 91,
|
|
k_cam_focus = 92,
|
|
k_cam_shutter_speed = 93,
|
|
k_scripting1 = 94, ///< Scripting related outputs
|
|
k_scripting2 = 95,
|
|
k_scripting3 = 96,
|
|
k_scripting4 = 97,
|
|
k_scripting5 = 98,
|
|
k_scripting6 = 99,
|
|
k_scripting7 = 100,
|
|
k_scripting8 = 101,
|
|
k_scripting9 = 102,
|
|
k_scripting10 = 103,
|
|
k_scripting11 = 104,
|
|
k_scripting12 = 105,
|
|
k_scripting13 = 106,
|
|
k_scripting14 = 107,
|
|
k_scripting15 = 108,
|
|
k_scripting16 = 109,
|
|
k_LED_neopixel1 = 120,
|
|
k_LED_neopixel2 = 121,
|
|
k_LED_neopixel3 = 122,
|
|
k_LED_neopixel4 = 123,
|
|
k_roll_out = 124,
|
|
k_pitch_out = 125,
|
|
k_thrust_out = 126,
|
|
k_yaw_out = 127,
|
|
k_wingsail_elevator = 128,
|
|
k_ProfiLED_1 = 129,
|
|
k_ProfiLED_2 = 130,
|
|
k_ProfiLED_3 = 131,
|
|
k_ProfiLED_Clock = 132,
|
|
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
|
|
};
|
|
|
|
// 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;
|
|
}
|
|
|
|
// return true if function is for a multicopter motor
|
|
static bool is_motor(SRV_Channel::Aux_servo_function_t function);
|
|
|
|
// return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
|
|
static bool should_e_stop(SRV_Channel::Aux_servo_function_t function);
|
|
|
|
// return the function of a channel
|
|
SRV_Channel::Aux_servo_function_t get_function(void) const {
|
|
return (SRV_Channel::Aux_servo_function_t)function.get();
|
|
}
|
|
|
|
// set and save function for channel. Used in upgrade of parameters in plane
|
|
void function_set_and_save(SRV_Channel::Aux_servo_function_t f) {
|
|
function.set_and_save(int8_t(f));
|
|
}
|
|
|
|
// set and save function for reversed. Used in upgrade of parameters in plane
|
|
void reversed_set_and_save_ifchanged(bool r) {
|
|
reversed.set_and_save_ifchanged(r?1:0);
|
|
}
|
|
|
|
// return true if the SERVOn_FUNCTION has been configured in
|
|
// either storage or a defaults file. This is used for upgrade of
|
|
// parameters in plane
|
|
bool function_configured(void) const {
|
|
return function.configured();
|
|
}
|
|
|
|
// specify that small rc input changes should be ignored during passthrough
|
|
// used by DO_SET_SERVO commands
|
|
void ignore_small_rcin_changes() { ign_small_rcin_changes = true; }
|
|
|
|
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_Int16 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;
|
|
|
|
// previous radio_in during pass-thru
|
|
int16_t previous_radio_in;
|
|
|
|
// specify that small rcinput changes should be ignored during passthrough
|
|
// used by DO_SET_SERVO commands
|
|
bool ign_small_rcin_changes;
|
|
};
|
|
|
|
/*
|
|
class SRV_Channels
|
|
*/
|
|
class SRV_Channels {
|
|
public:
|
|
friend class SRV_Channel;
|
|
|
|
// 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 specific function channel as a pwm value
|
|
static void set_output_pwm_chan(uint8_t chan, 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);
|
|
|
|
// get output channel mask for a function
|
|
static uint16_t get_output_channel_mask(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, float dt);
|
|
|
|
// call output_ch() on all channels
|
|
static void output_ch_all(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);
|
|
|
|
// set MIN/MAX parameters for a function
|
|
static void set_output_min_max(SRV_Channel::Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm);
|
|
|
|
// 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;
|
|
}
|
|
|
|
// setup IO failsafe for all channels to trim
|
|
static void setup_failsafe_trim_all_non_motors(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 the output value
|
|
static void set_trim_to_servo_out_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 servo out
|
|
static void copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output=false);
|
|
|
|
// copy radio_in to servo_out by channel mask
|
|
static void copy_radio_in_out_mask(uint16_t mask);
|
|
|
|
// setup failsafe for an auxiliary channel function, by pwm
|
|
static void set_failsafe_pwm(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);
|
|
|
|
// enable channels by mask
|
|
static void enable_by_mask(uint16_t mask);
|
|
|
|
// 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);
|
|
|
|
// set output refresh frequency on a servo function
|
|
static void set_rc_frequency(SRV_Channel::Aux_servo_function_t function, uint16_t frequency);
|
|
|
|
// control pass-thru of channels
|
|
void disable_passthrough(bool disable) {
|
|
disabled_passthrough = disable;
|
|
}
|
|
|
|
// 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<NUM_SERVO_CHANNELS?&channels[i]:nullptr;
|
|
}
|
|
|
|
// SERVO* parameters
|
|
static void upgrade_parameters(void);
|
|
|
|
// given a zero-based motor channel, return the k_motor function for that channel
|
|
static SRV_Channel::Aux_servo_function_t get_motor_function(uint8_t channel) {
|
|
if (channel < 8) {
|
|
return SRV_Channel::Aux_servo_function_t(SRV_Channel::k_motor1+channel);
|
|
}
|
|
return SRV_Channel::Aux_servo_function_t((SRV_Channel::k_motor9+(channel-8)));
|
|
}
|
|
|
|
static void cork();
|
|
|
|
static void push();
|
|
|
|
// disable output to a set of channels given by a mask. This is used by the AP_BLHeli code
|
|
static void set_disabled_channel_mask(uint16_t mask) { disabled_mask = mask; }
|
|
|
|
// add to mask of outputs which can do reverse thrust using digital controls
|
|
static void set_reversible_mask(uint16_t mask) {
|
|
reversible_mask |= mask;
|
|
}
|
|
|
|
// add to mask of outputs which use digital (non-PWM) output, such as DShot
|
|
static void set_digital_mask(uint16_t mask) {
|
|
digital_mask |= mask;
|
|
}
|
|
|
|
// Set E - stop
|
|
static void set_emergency_stop(bool state) {
|
|
emergency_stop = state;
|
|
}
|
|
|
|
// get E - stop
|
|
static bool get_emergency_stop() { return emergency_stop;}
|
|
|
|
// singleton for Lua
|
|
static SRV_Channels *get_singleton(void) {
|
|
return _singleton;
|
|
}
|
|
|
|
private:
|
|
struct {
|
|
bool k_throttle_reversible:1;
|
|
} flags;
|
|
|
|
static bool disabled_passthrough;
|
|
|
|
SRV_Channel::servo_mask_t trimmed_mask;
|
|
|
|
static Bitmask<SRV_Channel::k_nr_aux_servo_functions> function_mask;
|
|
static bool initialised;
|
|
|
|
// this static arrangement is to avoid having static objects in AP_Param tables
|
|
static SRV_Channel *channels;
|
|
static SRV_Channels *_singleton;
|
|
|
|
// support for Volz protocol
|
|
AP_Volz_Protocol volz;
|
|
static AP_Volz_Protocol *volz_ptr;
|
|
|
|
// support for SBUS protocol
|
|
AP_SBusOut sbus;
|
|
static AP_SBusOut *sbus_ptr;
|
|
|
|
// support for Robotis servo protocol
|
|
AP_RobotisServo robotis;
|
|
static AP_RobotisServo *robotis_ptr;
|
|
|
|
#if HAL_SUPPORT_RCOUT_SERIAL
|
|
// support for BLHeli protocol
|
|
AP_BLHeli blheli;
|
|
static AP_BLHeli *blheli_ptr;
|
|
#endif
|
|
static uint16_t disabled_mask;
|
|
|
|
// mask of outputs which use a digital output protocol, not
|
|
// PWM (eg. DShot)
|
|
static uint16_t digital_mask;
|
|
|
|
// mask of outputs which are digitally reversible (eg. DShot-3D)
|
|
static uint16_t reversible_mask;
|
|
|
|
SRV_Channel obj_channels[NUM_SERVO_CHANNELS];
|
|
|
|
static struct srv_function {
|
|
// mask of what channels this applies to
|
|
SRV_Channel::servo_mask_t channel_mask;
|
|
|
|
// scaled output for this function
|
|
int16_t output_scaled;
|
|
} functions[SRV_Channel::k_nr_aux_servo_functions];
|
|
|
|
AP_Int8 auto_trim;
|
|
AP_Int16 default_rate;
|
|
|
|
// return true if passthrough is disabled
|
|
static bool passthrough_disabled(void) {
|
|
return disabled_passthrough;
|
|
}
|
|
|
|
static bool emergency_stop;
|
|
};
|