2017-01-06 21:02:32 -04:00
|
|
|
/*
|
|
|
|
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
|
|
|
|
|
2021-04-28 17:50:49 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2017-01-06 21:02:32 -04:00
|
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#include <AP_Param/AP_Param.h>
|
|
|
|
#include <AP_Common/Bitmask.h>
|
2017-11-02 23:27:29 -03:00
|
|
|
#include <AP_Volz_Protocol/AP_Volz_Protocol.h>
|
2018-12-18 23:28:35 -04:00
|
|
|
#include <AP_RobotisServo/AP_RobotisServo.h>
|
2017-11-22 13:40:44 -04:00
|
|
|
#include <AP_SBusOut/AP_SBusOut.h>
|
2018-03-24 19:23:03 -03:00
|
|
|
#include <AP_BLHeli/AP_BLHeli.h>
|
2021-02-05 14:16:48 -04:00
|
|
|
#include <AP_FETtecOneWire/AP_FETtecOneWire.h>
|
2017-01-06 21:02:32 -04:00
|
|
|
|
2022-11-29 05:46:28 -04:00
|
|
|
#include "SRV_Channel_config.h"
|
|
|
|
|
2021-01-11 22:02:50 -04:00
|
|
|
static_assert(NUM_SERVO_CHANNELS <= 32, "More than 32 servos not supported");
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
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;
|
2017-11-02 23:27:29 -03:00
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// constructor
|
|
|
|
SRV_Channel(void);
|
|
|
|
|
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
|
|
|
|
typedef enum
|
|
|
|
{
|
2021-07-14 19:39:10 -03:00
|
|
|
k_GPIO = -1, ///< used as GPIO pin (input or output)
|
2022-04-19 14:17:10 -03:00
|
|
|
k_none = 0, ///< general use PWM output used by do-set-servo commands and lua scripts
|
2017-01-06 21:02:32 -04:00
|
|
|
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
|
2020-02-04 21:32:44 -04:00
|
|
|
k_egg_drop = 11, ///< egg drop, deprecated
|
2017-01-06 21:02:32 -04:00
|
|
|
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)
|
2017-07-01 03:01:32 -03:00
|
|
|
k_dspoilerLeft1 = 16, ///< differential spoiler 1 (left wing)
|
|
|
|
k_dspoilerRight1 = 17, ///< differential spoiler 1 (right wing)
|
2017-07-01 08:22:42 -03:00
|
|
|
k_aileron_with_input = 18, ///< aileron, with rc input, deprecated
|
2017-01-06 21:02:32 -04:00
|
|
|
k_elevator = 19, ///< elevator
|
2017-07-01 08:22:42 -03:00
|
|
|
k_elevator_with_input = 20, ///< elevator, with rc input, deprecated
|
2017-01-06 21:02:32 -04:00
|
|
|
k_rudder = 21, ///< secondary rudder channel
|
|
|
|
k_sprayer_pump = 22, ///< crop sprayer pump channel
|
|
|
|
k_sprayer_spinner = 23, ///< crop sprayer spinner channel
|
2017-06-10 19:44:04 -03:00
|
|
|
k_flaperon_left = 24, ///< flaperon, left wing
|
|
|
|
k_flaperon_right = 25, ///< flaperon, right wing
|
2017-01-06 21:02:32 -04:00
|
|
|
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
|
2020-08-04 22:51:48 -03:00
|
|
|
k_generator_control = 42, ///< state control for generator
|
2020-12-18 06:53:43 -04:00
|
|
|
k_tiltMotorRear = 45, ///<vectored thrust, rear tilt
|
|
|
|
k_tiltMotorRearLeft = 46, ///<vectored thrust, rear left tilt
|
|
|
|
k_tiltMotorRearRight = 47, ///<vectored thrust, rear right tilt
|
2017-01-06 21:02:32 -04:00
|
|
|
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,
|
2020-02-04 21:32:44 -04:00
|
|
|
k_choke = 68, /// not used
|
2017-01-06 21:02:32 -04:00
|
|
|
k_starter = 69,
|
|
|
|
k_throttle = 70,
|
2017-01-08 18:29:00 -04:00
|
|
|
k_tracker_yaw = 71, ///< antennatracker yaw
|
|
|
|
k_tracker_pitch = 72, ///< antennatracker pitch
|
2017-02-12 21:03:03 -04:00
|
|
|
k_throttleLeft = 73,
|
|
|
|
k_throttleRight = 74,
|
2017-04-09 20:49:35 -03:00
|
|
|
k_tiltMotorLeft = 75, ///< vectored thrust, left tilt
|
|
|
|
k_tiltMotorRight = 76, ///< vectored thrust, right tilt
|
2017-04-19 08:32:47 -03:00
|
|
|
k_elevon_left = 77,
|
|
|
|
k_elevon_right = 78,
|
|
|
|
k_vtail_left = 79,
|
|
|
|
k_vtail_right = 80,
|
2017-04-17 07:04:11 -03:00
|
|
|
k_boost_throttle = 81, ///< vertical booster throttle
|
2017-05-18 11:16:51 -03:00
|
|
|
k_motor9 = 82,
|
|
|
|
k_motor10 = 83,
|
|
|
|
k_motor11 = 84,
|
|
|
|
k_motor12 = 85,
|
2017-07-01 03:01:32 -03:00
|
|
|
k_dspoilerLeft2 = 86, ///< differential spoiler 2 (left wing)
|
|
|
|
k_dspoilerRight2 = 87, ///< differential spoiler 2 (right wing)
|
2017-10-04 23:21:03 -03:00
|
|
|
k_winch = 88,
|
2018-08-21 23:46:13 -03:00
|
|
|
k_mainsail_sheet = 89, ///< Main Sail control via sheet
|
2018-10-03 21:18:51 -03:00
|
|
|
k_cam_iso = 90,
|
|
|
|
k_cam_aperture = 91,
|
|
|
|
k_cam_focus = 92,
|
|
|
|
k_cam_shutter_speed = 93,
|
2018-10-30 03:00:53 -03:00
|
|
|
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,
|
2018-11-12 11:41:22 -04:00
|
|
|
k_airbrake = 110,
|
2019-03-06 22:13:24 -04:00
|
|
|
k_LED_neopixel1 = 120,
|
|
|
|
k_LED_neopixel2 = 121,
|
|
|
|
k_LED_neopixel3 = 122,
|
|
|
|
k_LED_neopixel4 = 123,
|
2019-10-14 20:25:33 -03:00
|
|
|
k_roll_out = 124,
|
|
|
|
k_pitch_out = 125,
|
|
|
|
k_thrust_out = 126,
|
|
|
|
k_yaw_out = 127,
|
2019-09-27 15:50:27 -03:00
|
|
|
k_wingsail_elevator = 128,
|
2020-02-22 19:52:57 -04:00
|
|
|
k_ProfiLED_1 = 129,
|
|
|
|
k_ProfiLED_2 = 130,
|
|
|
|
k_ProfiLED_3 = 131,
|
|
|
|
k_ProfiLED_Clock = 132,
|
2020-07-20 04:32:21 -03:00
|
|
|
k_winch_clutch = 133,
|
2020-08-30 23:35:24 -03:00
|
|
|
k_min = 134, // always outputs SERVOn_MIN
|
|
|
|
k_trim = 135, // always outputs SERVOn_TRIM
|
|
|
|
k_max = 136, // always outputs SERVOn_MAX
|
2021-04-27 16:49:19 -03:00
|
|
|
k_mast_rotation = 137,
|
2022-02-12 17:55:37 -04:00
|
|
|
k_alarm = 138,
|
|
|
|
k_alarm_inverted = 139,
|
2022-08-05 17:07:04 -03:00
|
|
|
k_rcin1_mapped = 140,
|
|
|
|
k_rcin2_mapped = 141,
|
|
|
|
k_rcin3_mapped = 142,
|
|
|
|
k_rcin4_mapped = 143,
|
|
|
|
k_rcin5_mapped = 144,
|
|
|
|
k_rcin6_mapped = 145,
|
|
|
|
k_rcin7_mapped = 146,
|
|
|
|
k_rcin8_mapped = 147,
|
|
|
|
k_rcin9_mapped = 148,
|
|
|
|
k_rcin10_mapped = 149,
|
|
|
|
k_rcin11_mapped = 150,
|
|
|
|
k_rcin12_mapped = 151,
|
|
|
|
k_rcin13_mapped = 152,
|
|
|
|
k_rcin14_mapped = 153,
|
|
|
|
k_rcin15_mapped = 154,
|
|
|
|
k_rcin16_mapped = 155,
|
2017-01-06 21:02:32 -04:00
|
|
|
k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one)
|
|
|
|
} Aux_servo_function_t;
|
|
|
|
|
2021-12-14 23:58:47 -04:00
|
|
|
// check if a function is valid for indexing into functions
|
|
|
|
static bool valid_function(Aux_servo_function_t fn) {
|
2022-04-19 14:17:10 -03:00
|
|
|
return fn >= k_none && fn < k_nr_aux_servo_functions;
|
2021-12-14 23:58:47 -04:00
|
|
|
}
|
|
|
|
bool valid_function(void) const {
|
|
|
|
return valid_function(function);
|
|
|
|
}
|
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// used to get min/max/trim limit value based on reverse
|
2019-11-24 21:49:24 -04:00
|
|
|
enum class Limit {
|
|
|
|
TRIM,
|
|
|
|
MIN,
|
|
|
|
MAX,
|
|
|
|
ZERO_PWM
|
2017-01-06 21:02:32 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
// set the output value as a pwm value
|
2020-06-07 17:04:56 -03:00
|
|
|
void set_output_pwm(uint16_t pwm, bool force = false);
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// get the output value as a pwm value
|
|
|
|
uint16_t get_output_pwm(void) const { return output_pwm; }
|
|
|
|
|
2020-08-07 22:23:56 -03:00
|
|
|
// set normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
|
|
|
|
void set_output_norm(float value);
|
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// 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 {
|
2021-02-03 10:04:39 -04:00
|
|
|
return reversed != 0;
|
2017-01-06 21:02:32 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
}
|
|
|
|
|
2017-05-18 11:16:51 -03:00
|
|
|
// return true if function is for a multicopter motor
|
|
|
|
static bool is_motor(SRV_Channel::Aux_servo_function_t function);
|
|
|
|
|
2019-01-20 21:36:10 -04:00
|
|
|
// 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);
|
|
|
|
|
2021-09-16 14:23:06 -03:00
|
|
|
// return true if function is for a control surface
|
|
|
|
static bool is_control_surface(SRV_Channel::Aux_servo_function_t function);
|
|
|
|
|
2017-07-06 01:12:21 -03:00
|
|
|
// 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();
|
|
|
|
}
|
|
|
|
|
2021-12-08 19:50:32 -04:00
|
|
|
// return the motor number of a channel, or -1 if not a motor
|
|
|
|
int8_t get_motor_num(void) const;
|
|
|
|
|
2017-07-06 01:12:21 -03:00
|
|
|
// 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();
|
|
|
|
}
|
2019-06-02 22:57:01 -03:00
|
|
|
|
|
|
|
// 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; }
|
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
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;
|
2021-11-29 02:56:33 -04:00
|
|
|
AP_Enum16<Aux_servo_function_t> function;
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// 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;
|
2017-07-25 00:23:09 -03:00
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// 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
|
2021-09-18 14:54:52 -03:00
|
|
|
uint16_t pwm_from_range(float scaled_value) const;
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// convert a -angle_max..angle_max to a pwm
|
2021-09-18 14:54:52 -03:00
|
|
|
uint16_t pwm_from_angle(float scaled_value) const;
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// convert a scaled output to a pwm value
|
2021-09-18 14:54:52 -03:00
|
|
|
void calc_pwm(float output_scaled);
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// 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
|
2019-11-24 21:49:24 -04:00
|
|
|
uint16_t get_limit_pwm(Limit limit) const;
|
2017-01-06 21:02:32 -04:00
|
|
|
|
2020-08-16 23:08:42 -03:00
|
|
|
// get normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
|
2017-01-06 21:02:32 -04:00
|
|
|
float get_output_norm(void);
|
2017-07-25 00:23:09 -03:00
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// a bitmask type wide enough for NUM_SERVO_CHANNELS
|
2021-01-11 22:02:50 -04:00
|
|
|
typedef uint32_t servo_mask_t;
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// mask of channels where we have a output_pwm value. Cleared when a
|
|
|
|
// scaled value is written.
|
|
|
|
static servo_mask_t have_pwm_mask;
|
2019-06-02 22:57:01 -03:00
|
|
|
|
|
|
|
// 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;
|
2020-06-07 17:04:56 -03:00
|
|
|
|
|
|
|
// if true we should ignore all imputs on this channel
|
|
|
|
bool override_active;
|
|
|
|
|
|
|
|
void set_override(bool b) {override_active = b;};
|
2017-01-06 21:02:32 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
/*
|
|
|
|
class SRV_Channels
|
|
|
|
*/
|
|
|
|
class SRV_Channels {
|
|
|
|
public:
|
2017-01-07 02:13:54 -04:00
|
|
|
friend class SRV_Channel;
|
2017-07-25 00:23:09 -03:00
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// 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);
|
|
|
|
|
2017-04-17 03:53:05 -03:00
|
|
|
// set output value for a specific function channel as a pwm value
|
|
|
|
static void set_output_pwm_chan(uint8_t chan, uint16_t value);
|
2017-07-25 00:23:09 -03:00
|
|
|
|
2020-05-24 23:45:17 -03:00
|
|
|
// set output value for a specific function channel as a pwm value for specified override time in ms
|
|
|
|
static void set_output_pwm_chan_timeout(uint8_t chan, uint16_t value, uint16_t timeout_ms);
|
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// set output value for a function channel as a scaled value. This
|
|
|
|
// calls calc_pwm() to also set the pwm value
|
2021-09-18 14:54:52 -03:00
|
|
|
static void set_output_scaled(SRV_Channel::Aux_servo_function_t function, float value);
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// get scaled output for the given function type.
|
2021-09-18 14:54:52 -03:00
|
|
|
static float get_output_scaled(SRV_Channel::Aux_servo_function_t function);
|
2017-01-06 21:02:32 -04:00
|
|
|
|
2022-01-16 09:06:13 -04:00
|
|
|
// get slew limited scaled output for the given function type
|
|
|
|
static float get_slew_limited_output_scaled(SRV_Channel::Aux_servo_function_t function);
|
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// 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);
|
|
|
|
|
2020-08-16 23:08:42 -03:00
|
|
|
// get normalised output (-1 to 1 with 0 at mid point of servo_min/servo_max)
|
|
|
|
// Value is taken from pwm value. Returns zero on error.
|
2017-01-06 21:02:32 -04:00
|
|
|
static float get_output_norm(SRV_Channel::Aux_servo_function_t function);
|
|
|
|
|
2020-08-07 22:23:56 -03:00
|
|
|
// set normalised output (-1 to 1 with 0 at mid point of servo_min/servo_max) for the given function
|
|
|
|
static void set_output_norm(SRV_Channel::Aux_servo_function_t function, float value);
|
|
|
|
|
2017-04-17 05:15:24 -03:00
|
|
|
// get output channel mask for a function
|
2021-01-11 22:02:50 -04:00
|
|
|
static uint32_t get_output_channel_mask(SRV_Channel::Aux_servo_function_t function);
|
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// limit slew rate to given limit in percent per second
|
2022-01-16 09:06:13 -04:00
|
|
|
static void set_slew_rate(SRV_Channel::Aux_servo_function_t function, float slew_rate, uint16_t range, float dt);
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// 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);
|
|
|
|
|
2018-05-20 23:58:12 -03:00
|
|
|
// 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);
|
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// save trims
|
|
|
|
void save_trim(void);
|
|
|
|
|
|
|
|
// setup IO failsafe for all channels to trim
|
2018-08-22 04:18:36 -03:00
|
|
|
static void setup_failsafe_trim_all_non_motors(void);
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// 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);
|
|
|
|
|
2017-07-01 07:04:30 -03:00
|
|
|
// 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);
|
2017-07-25 00:23:09 -03:00
|
|
|
|
2021-08-29 08:53:20 -03:00
|
|
|
// set the trim for a function channel to min of the channel honnoring reverse unless ignore_reversed is true
|
|
|
|
static void set_trim_to_min_for(SRV_Channel::Aux_servo_function_t function, bool ignore_reversed = false);
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// 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);
|
|
|
|
|
2017-07-10 03:59:53 -03:00
|
|
|
// copy radio_in to servo out
|
2017-01-06 21:02:32 -04:00
|
|
|
static void copy_radio_in_out(SRV_Channel::Aux_servo_function_t function, bool do_input_output=false);
|
|
|
|
|
2017-07-10 03:59:53 -03:00
|
|
|
// copy radio_in to servo_out by channel mask
|
2021-01-11 22:02:50 -04:00
|
|
|
static void copy_radio_in_out_mask(uint32_t mask);
|
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// setup failsafe for an auxiliary channel function, by pwm
|
2017-03-16 10:19:01 -03:00
|
|
|
static void set_failsafe_pwm(SRV_Channel::Aux_servo_function_t function, uint16_t pwm);
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// setup failsafe for an auxiliary channel function
|
2019-11-24 21:49:24 -04:00
|
|
|
static void set_failsafe_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::Limit limit);
|
2017-01-06 21:02:32 -04:00
|
|
|
|
2019-11-24 21:49:24 -04:00
|
|
|
// set servo to a Limit
|
|
|
|
static void set_output_limit(SRV_Channel::Aux_servo_function_t function, SRV_Channel::Limit limit);
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// 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);
|
|
|
|
|
2017-04-29 05:33:49 -03:00
|
|
|
// enable channels by mask
|
2021-01-11 22:02:50 -04:00
|
|
|
static void enable_by_mask(uint32_t mask);
|
2017-07-25 00:23:09 -03:00
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// 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
|
2023-03-28 01:13:42 -03:00
|
|
|
static SRV_Channel *get_channel_for(SRV_Channel::Aux_servo_function_t function);
|
2017-01-06 21:02:32 -04:00
|
|
|
|
|
|
|
// 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);
|
2017-04-10 01:50:40 -03:00
|
|
|
|
|
|
|
// set output refresh frequency on a servo function
|
|
|
|
static void set_rc_frequency(SRV_Channel::Aux_servo_function_t function, uint16_t frequency);
|
2017-07-25 00:23:09 -03:00
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// 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);
|
2017-07-25 00:23:09 -03:00
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// calculate PWM for all channels
|
|
|
|
static void calc_pwm(void);
|
|
|
|
|
2021-04-28 17:50:49 -03:00
|
|
|
// return the ESC type for dshot commands
|
|
|
|
static AP_HAL::RCOutput::DshotEscType get_dshot_esc_type() { return AP_HAL::RCOutput::DshotEscType(_singleton->dshot_esc_type.get()); }
|
|
|
|
|
2022-09-05 14:06:42 -03:00
|
|
|
static uint8_t get_dshot_rate() { return _singleton->dshot_rate.get(); }
|
2023-06-22 20:10:42 -03:00
|
|
|
|
|
|
|
static uint8_t get_rc_fs_mask() { return _singleton->rc_fs_mask.get(); }
|
2022-09-05 14:06:42 -03:00
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
static SRV_Channel *srv_channel(uint8_t i) {
|
2021-04-29 02:12:58 -03:00
|
|
|
#if NUM_SERVO_CHANNELS > 0
|
2017-01-06 21:02:32 -04:00
|
|
|
return i<NUM_SERVO_CHANNELS?&channels[i]:nullptr;
|
2021-04-29 02:12:58 -03:00
|
|
|
#else
|
|
|
|
return nullptr;
|
|
|
|
#endif
|
2017-01-06 21:02:32 -04:00
|
|
|
}
|
|
|
|
|
2020-01-13 00:45:32 -04:00
|
|
|
// SERVO* parameters
|
|
|
|
static void upgrade_parameters(void);
|
2017-07-25 00:23:09 -03:00
|
|
|
|
2017-10-23 00:16:14 -03:00
|
|
|
// 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)));
|
|
|
|
}
|
|
|
|
|
2017-11-02 23:27:29 -03:00
|
|
|
static void cork();
|
|
|
|
|
|
|
|
static void push();
|
|
|
|
|
2022-04-19 14:17:10 -03:00
|
|
|
// disable PWM output to a set of channels given by a mask. This is used by the AP_BLHeli code
|
2021-01-11 22:02:50 -04:00
|
|
|
static void set_disabled_channel_mask(uint32_t mask) { disabled_mask = mask; }
|
|
|
|
static uint32_t get_disabled_channel_mask() { return disabled_mask; }
|
2018-04-01 03:01:37 -03:00
|
|
|
|
2021-05-06 10:13:36 -03:00
|
|
|
// add to mask of outputs which use digital (non-PWM) output and optionally can reverse thrust, such as DShot
|
2021-01-11 22:02:50 -04:00
|
|
|
static void set_digital_outputs(uint32_t dig_mask, uint32_t rev_mask);
|
2021-05-01 12:39:32 -03:00
|
|
|
|
|
|
|
// return true if all of the outputs in mask are digital
|
2021-01-11 22:02:50 -04:00
|
|
|
static bool have_digital_outputs(uint32_t mask) { return mask != 0 && (mask & digital_mask) == mask; }
|
2019-01-20 21:36:10 -04:00
|
|
|
|
2021-05-15 05:48:36 -03:00
|
|
|
// return true if any of the outputs are digital
|
|
|
|
static bool have_digital_outputs() { return digital_mask != 0; }
|
|
|
|
|
2019-01-20 21:36:10 -04:00
|
|
|
// Set E - stop
|
2022-08-19 12:06:24 -03:00
|
|
|
static void set_emergency_stop(bool state);
|
2019-01-20 21:36:10 -04:00
|
|
|
|
|
|
|
// get E - stop
|
|
|
|
static bool get_emergency_stop() { return emergency_stop;}
|
|
|
|
|
2019-10-30 18:23:01 -03:00
|
|
|
// singleton for Lua
|
|
|
|
static SRV_Channels *get_singleton(void) {
|
|
|
|
return _singleton;
|
|
|
|
}
|
|
|
|
|
2020-09-10 08:17:56 -03:00
|
|
|
static void zero_rc_outputs();
|
|
|
|
|
2021-03-02 18:18:47 -04:00
|
|
|
// initialize before any call to push
|
2022-09-05 14:06:42 -03:00
|
|
|
static void init(uint32_t motor_mask = 0, AP_HAL::RCOutput::output_mode mode = AP_HAL::RCOutput::MODE_PWM_NONE);
|
2021-03-02 18:18:47 -04:00
|
|
|
|
2021-07-14 19:39:10 -03:00
|
|
|
// return true if a channel is set to type GPIO
|
2021-08-03 21:24:01 -03:00
|
|
|
static bool is_GPIO(uint8_t channel);
|
2021-07-14 19:39:10 -03:00
|
|
|
|
2022-02-12 17:55:37 -04:00
|
|
|
// return true if a channel is set to type alarm
|
|
|
|
static bool is_alarm(uint8_t channel) {
|
|
|
|
return channel_function(channel) == SRV_Channel::k_alarm;
|
|
|
|
}
|
|
|
|
|
|
|
|
// return true if a channel is set to type alarm inverted
|
|
|
|
static bool is_alarm_inverted(uint8_t channel) {
|
|
|
|
return channel_function(channel) == SRV_Channel::k_alarm_inverted;
|
|
|
|
}
|
|
|
|
|
2023-03-16 20:11:07 -03:00
|
|
|
// return true if 32 channels are enabled
|
|
|
|
static bool have_32_channels() {
|
|
|
|
#if NUM_SERVO_CHANNELS >= 17
|
|
|
|
return _singleton->enable_32_channels.get() > 0;
|
|
|
|
#else
|
|
|
|
return false;
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
private:
|
|
|
|
|
|
|
|
static bool disabled_passthrough;
|
|
|
|
|
2017-01-07 01:53:40 -04:00
|
|
|
SRV_Channel::servo_mask_t trimmed_mask;
|
2017-01-06 21:02:32 -04:00
|
|
|
|
2019-04-11 09:13:23 -03:00
|
|
|
static Bitmask<SRV_Channel::k_nr_aux_servo_functions> function_mask;
|
2017-01-06 21:02:32 -04:00
|
|
|
static bool initialised;
|
2017-07-25 00:23:09 -03:00
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
// this static arrangement is to avoid having static objects in AP_Param tables
|
|
|
|
static SRV_Channel *channels;
|
2019-02-10 00:59:44 -04:00
|
|
|
static SRV_Channels *_singleton;
|
2017-11-02 23:27:29 -03:00
|
|
|
|
2022-04-08 04:20:50 -03:00
|
|
|
#if AP_VOLZ_ENABLED
|
2017-11-02 23:27:29 -03:00
|
|
|
// support for Volz protocol
|
2017-12-12 21:06:15 -04:00
|
|
|
AP_Volz_Protocol volz;
|
2017-11-02 23:27:29 -03:00
|
|
|
static AP_Volz_Protocol *volz_ptr;
|
2022-04-08 04:20:50 -03:00
|
|
|
#endif
|
2017-11-02 23:27:29 -03:00
|
|
|
|
2023-06-15 05:25:10 -03:00
|
|
|
#if AP_SBUSOUTPUT_ENABLED
|
2017-11-22 13:40:44 -04:00
|
|
|
// support for SBUS protocol
|
2017-12-12 21:06:15 -04:00
|
|
|
AP_SBusOut sbus;
|
2017-11-22 13:40:44 -04:00
|
|
|
static AP_SBusOut *sbus_ptr;
|
2023-06-15 05:25:10 -03:00
|
|
|
#endif
|
2017-11-22 13:40:44 -04:00
|
|
|
|
2022-04-08 04:20:57 -03:00
|
|
|
#if AP_ROBOTISSERVO_ENABLED
|
2018-12-18 23:28:35 -04:00
|
|
|
// support for Robotis servo protocol
|
|
|
|
AP_RobotisServo robotis;
|
|
|
|
static AP_RobotisServo *robotis_ptr;
|
2022-04-08 04:20:57 -03:00
|
|
|
#endif
|
2021-12-06 22:46:32 -04:00
|
|
|
|
2018-04-01 22:16:44 -03:00
|
|
|
#if HAL_SUPPORT_RCOUT_SERIAL
|
2018-03-24 19:23:03 -03:00
|
|
|
// support for BLHeli protocol
|
|
|
|
AP_BLHeli blheli;
|
|
|
|
static AP_BLHeli *blheli_ptr;
|
2018-04-01 22:16:44 -03:00
|
|
|
#endif
|
2021-02-05 14:16:48 -04:00
|
|
|
|
2021-11-23 00:52:22 -04:00
|
|
|
#if AP_FETTEC_ONEWIRE_ENABLED
|
2021-02-05 14:16:48 -04:00
|
|
|
AP_FETtecOneWire fetteconwire;
|
|
|
|
static AP_FETtecOneWire *fetteconwire_ptr;
|
2021-11-23 00:52:22 -04:00
|
|
|
#endif // AP_FETTEC_ONEWIRE_ENABLED
|
2020-12-01 07:30:12 -04:00
|
|
|
|
2022-04-19 14:17:10 -03:00
|
|
|
// mask of disabled channels
|
2021-01-11 22:02:50 -04:00
|
|
|
static uint32_t disabled_mask;
|
2018-11-09 06:26:38 -04:00
|
|
|
|
|
|
|
// mask of outputs which use a digital output protocol, not
|
|
|
|
// PWM (eg. DShot)
|
2021-01-11 22:02:50 -04:00
|
|
|
static uint32_t digital_mask;
|
2018-03-24 19:23:03 -03:00
|
|
|
|
2018-11-09 06:26:38 -04:00
|
|
|
// mask of outputs which are digitally reversible (eg. DShot-3D)
|
2021-01-11 22:02:50 -04:00
|
|
|
static uint32_t reversible_mask;
|
2018-11-09 06:26:38 -04:00
|
|
|
|
2022-01-02 13:40:16 -04:00
|
|
|
// mask of channels with invalid funtions, eg GPIO
|
|
|
|
static uint32_t invalid_mask;
|
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
SRV_Channel obj_channels[NUM_SERVO_CHANNELS];
|
|
|
|
|
2020-05-24 23:45:17 -03:00
|
|
|
// override loop counter
|
|
|
|
static uint16_t override_counter[NUM_SERVO_CHANNELS];
|
|
|
|
|
2017-01-06 21:02:32 -04:00
|
|
|
static struct srv_function {
|
|
|
|
// mask of what channels this applies to
|
|
|
|
SRV_Channel::servo_mask_t channel_mask;
|
|
|
|
|
|
|
|
// scaled output for this function
|
2021-09-18 14:54:52 -03:00
|
|
|
float output_scaled;
|
2017-01-06 21:02:32 -04:00
|
|
|
} functions[SRV_Channel::k_nr_aux_servo_functions];
|
|
|
|
|
|
|
|
AP_Int8 auto_trim;
|
2017-07-16 20:19:45 -03:00
|
|
|
AP_Int16 default_rate;
|
2021-03-02 18:18:47 -04:00
|
|
|
AP_Int8 dshot_rate;
|
2021-04-28 17:50:49 -03:00
|
|
|
AP_Int8 dshot_esc_type;
|
2021-08-03 21:24:01 -03:00
|
|
|
AP_Int32 gpio_mask;
|
2023-06-22 20:10:42 -03:00
|
|
|
AP_Int32 rc_fs_mask;
|
2022-05-15 18:51:34 -03:00
|
|
|
#if NUM_SERVO_CHANNELS >= 17
|
|
|
|
AP_Int8 enable_32_channels;
|
|
|
|
#endif
|
2017-01-06 21:02:32 -04:00
|
|
|
|
2017-01-07 01:53:40 -04:00
|
|
|
// return true if passthrough is disabled
|
|
|
|
static bool passthrough_disabled(void) {
|
|
|
|
return disabled_passthrough;
|
|
|
|
}
|
2019-01-20 21:36:10 -04:00
|
|
|
|
|
|
|
static bool emergency_stop;
|
2020-05-24 23:45:17 -03:00
|
|
|
|
2022-01-16 09:06:13 -04:00
|
|
|
// linked list for slew rate handling
|
|
|
|
struct slew_list {
|
|
|
|
slew_list(SRV_Channel::Aux_servo_function_t _func) : func(_func) {};
|
|
|
|
const SRV_Channel::Aux_servo_function_t func;
|
|
|
|
float last_scaled_output;
|
|
|
|
float max_change;
|
|
|
|
slew_list * next;
|
|
|
|
};
|
|
|
|
static slew_list *_slew;
|
|
|
|
|
2020-05-24 23:45:17 -03:00
|
|
|
// semaphore for multi-thread use of override_counter array
|
|
|
|
HAL_Semaphore override_counter_sem;
|
2017-01-06 21:02:32 -04:00
|
|
|
};
|