Line data Source code
1 : /*
2 : control of servo output ranges, trim and servo reversal. This can
3 : optionally be used to provide separation of input and output channel
4 : ranges so that RCn_MIN, RCn_MAX, RCn_TRIM and RCn_REV only apply to
5 : the input side of RC_Channel
6 :
7 : It works by running servo output calculations as normal, then
8 : re-mapping the output according to the servo MIN/MAX/TRIM/REV from
9 : this object
10 :
11 : Only 4 channels of ranges are defined as those match the input
12 : channels for R/C sticks
13 : */
14 : #pragma once
15 :
16 : #include <AP_HAL/AP_HAL.h>
17 : #include <AP_Common/AP_Common.h>
18 : #include <AP_Param/AP_Param.h>
19 : #include <AP_Common/Bitmask.h>
20 : #include <AP_Volz_Protocol/AP_Volz_Protocol.h>
21 : #include <AP_RobotisServo/AP_RobotisServo.h>
22 : #include <AP_SBusOut/AP_SBusOut.h>
23 : #include <AP_BLHeli/AP_BLHeli.h>
24 : #include <AP_FETtecOneWire/AP_FETtecOneWire.h>
25 :
26 : #include "SRV_Channel_config.h"
27 :
28 : #ifndef ACTUATOR_CHANNELS
29 : #define ACTUATOR_CHANNELS 6
30 : #endif
31 :
32 : #define ACTUATOR_DEFAULT_INCREMENT 0.01
33 :
34 : static_assert(NUM_SERVO_CHANNELS <= 32, "More than 32 servos not supported");
35 :
36 : class SRV_Channels;
37 :
38 : /*
39 : class SRV_Channel. The class SRV_Channels contains an array of
40 : SRV_Channel objects. This is done to fit within the AP_Param limit
41 : of 64 parameters per object.
42 : */
43 : class SRV_Channel {
44 : public:
45 : friend class SRV_Channels;
46 :
47 : // constructor
48 : SRV_Channel(void);
49 :
50 : static const struct AP_Param::GroupInfo var_info[];
51 :
52 : typedef enum
53 : {
54 : k_GPIO = -1, ///< used as GPIO pin (input or output)
55 : k_none = 0, ///< general use PWM output used by do-set-servo commands and lua scripts
56 : k_manual = 1, ///< manual, just pass-thru the RC in signal
57 : k_flap = 2, ///< flap
58 : k_flap_auto = 3, ///< flap automated
59 : k_aileron = 4, ///< aileron
60 : k_unused1 = 5, ///< unused function
61 : k_mount_pan = 6, ///< mount yaw (pan)
62 : k_mount_tilt = 7, ///< mount pitch (tilt)
63 : k_mount_roll = 8, ///< mount roll
64 : k_mount_open = 9, ///< mount open (deploy) / close (retract)
65 : k_cam_trigger = 10, ///< camera trigger
66 : k_egg_drop = 11, ///< egg drop, deprecated
67 : k_mount2_pan = 12, ///< mount2 yaw (pan)
68 : k_mount2_tilt = 13, ///< mount2 pitch (tilt)
69 : k_mount2_roll = 14, ///< mount2 roll
70 : k_mount2_open = 15, ///< mount2 open (deploy) / close (retract)
71 : k_dspoilerLeft1 = 16, ///< differential spoiler 1 (left wing)
72 : k_dspoilerRight1 = 17, ///< differential spoiler 1 (right wing)
73 : k_aileron_with_input = 18, ///< aileron, with rc input, deprecated
74 : k_elevator = 19, ///< elevator
75 : k_elevator_with_input = 20, ///< elevator, with rc input, deprecated
76 : k_rudder = 21, ///< secondary rudder channel
77 : k_sprayer_pump = 22, ///< crop sprayer pump channel
78 : k_sprayer_spinner = 23, ///< crop sprayer spinner channel
79 : k_flaperon_left = 24, ///< flaperon, left wing
80 : k_flaperon_right = 25, ///< flaperon, right wing
81 : k_steering = 26, ///< ground steering, used to separate from rudder
82 : k_parachute_release = 27, ///< parachute release
83 : k_gripper = 28, ///< gripper
84 : k_landing_gear_control = 29, ///< landing gear controller
85 : k_engine_run_enable = 30, ///< engine kill switch, used for gas airplanes and helicopters
86 : k_heli_rsc = 31, ///< helicopter RSC output
87 : k_heli_tail_rsc = 32, ///< helicopter tail RSC output
88 : k_motor1 = 33, ///< these allow remapping of copter motors
89 : k_motor2 = 34,
90 : k_motor3 = 35,
91 : k_motor4 = 36,
92 : k_motor5 = 37,
93 : k_motor6 = 38,
94 : k_motor7 = 39,
95 : k_motor8 = 40,
96 : k_motor_tilt = 41, ///< tiltrotor motor tilt control
97 : k_generator_control = 42, ///< state control for generator
98 : k_tiltMotorRear = 45, ///<vectored thrust, rear tilt
99 : k_tiltMotorRearLeft = 46, ///<vectored thrust, rear left tilt
100 : k_tiltMotorRearRight = 47, ///<vectored thrust, rear right tilt
101 : k_rcin1 = 51, ///< these are for pass-thru from arbitrary rc inputs
102 : k_rcin2 = 52,
103 : k_rcin3 = 53,
104 : k_rcin4 = 54,
105 : k_rcin5 = 55,
106 : k_rcin6 = 56,
107 : k_rcin7 = 57,
108 : k_rcin8 = 58,
109 : k_rcin9 = 59,
110 : k_rcin10 = 60,
111 : k_rcin11 = 61,
112 : k_rcin12 = 62,
113 : k_rcin13 = 63,
114 : k_rcin14 = 64,
115 : k_rcin15 = 65,
116 : k_rcin16 = 66,
117 : k_ignition = 67,
118 : k_choke = 68, /// not used
119 : k_starter = 69,
120 : k_throttle = 70,
121 : k_tracker_yaw = 71, ///< antennatracker yaw
122 : k_tracker_pitch = 72, ///< antennatracker pitch
123 : k_throttleLeft = 73,
124 : k_throttleRight = 74,
125 : k_tiltMotorLeft = 75, ///< vectored thrust, left tilt
126 : k_tiltMotorRight = 76, ///< vectored thrust, right tilt
127 : k_elevon_left = 77,
128 : k_elevon_right = 78,
129 : k_vtail_left = 79,
130 : k_vtail_right = 80,
131 : k_boost_throttle = 81, ///< vertical booster throttle
132 : k_motor9 = 82,
133 : k_motor10 = 83,
134 : k_motor11 = 84,
135 : k_motor12 = 85,
136 : k_dspoilerLeft2 = 86, ///< differential spoiler 2 (left wing)
137 : k_dspoilerRight2 = 87, ///< differential spoiler 2 (right wing)
138 : k_winch = 88,
139 : k_mainsail_sheet = 89, ///< Main Sail control via sheet
140 : k_cam_iso = 90,
141 : k_cam_aperture = 91,
142 : k_cam_focus = 92,
143 : k_cam_shutter_speed = 93,
144 : k_scripting1 = 94, ///< Scripting related outputs
145 : k_scripting2 = 95,
146 : k_scripting3 = 96,
147 : k_scripting4 = 97,
148 : k_scripting5 = 98,
149 : k_scripting6 = 99,
150 : k_scripting7 = 100,
151 : k_scripting8 = 101,
152 : k_scripting9 = 102,
153 : k_scripting10 = 103,
154 : k_scripting11 = 104,
155 : k_scripting12 = 105,
156 : k_scripting13 = 106,
157 : k_scripting14 = 107,
158 : k_scripting15 = 108,
159 : k_scripting16 = 109,
160 : k_airbrake = 110,
161 : k_LED_neopixel1 = 120,
162 : k_LED_neopixel2 = 121,
163 : k_LED_neopixel3 = 122,
164 : k_LED_neopixel4 = 123,
165 : k_roll_out = 124,
166 : k_pitch_out = 125,
167 : k_thrust_out = 126,
168 : k_yaw_out = 127,
169 : k_wingsail_elevator = 128,
170 : k_ProfiLED_1 = 129,
171 : k_ProfiLED_2 = 130,
172 : k_ProfiLED_3 = 131,
173 : k_ProfiLED_Clock = 132,
174 : k_winch_clutch = 133,
175 : k_min = 134, // always outputs SERVOn_MIN
176 : k_trim = 135, // always outputs SERVOn_TRIM
177 : k_max = 136, // always outputs SERVOn_MAX
178 : k_mast_rotation = 137,
179 : k_alarm = 138,
180 : k_alarm_inverted = 139,
181 : k_rcin1_mapped = 140,
182 : k_rcin2_mapped = 141,
183 : k_rcin3_mapped = 142,
184 : k_rcin4_mapped = 143,
185 : k_rcin5_mapped = 144,
186 : k_rcin6_mapped = 145,
187 : k_rcin7_mapped = 146,
188 : k_rcin8_mapped = 147,
189 : k_rcin9_mapped = 148,
190 : k_rcin10_mapped = 149,
191 : k_rcin11_mapped = 150,
192 : k_rcin12_mapped = 151,
193 : k_rcin13_mapped = 152,
194 : k_rcin14_mapped = 153,
195 : k_rcin15_mapped = 154,
196 : k_rcin16_mapped = 155,
197 : k_lift_release = 156,
198 : k_motor13 = 160,
199 : k_motor14 = 161,
200 : k_motor15 = 162,
201 : k_motor16 = 163,
202 : k_motor17 = 164,
203 : k_motor18 = 165,
204 : k_motor19 = 166,
205 : k_motor20 = 167,
206 : k_motor21 = 168,
207 : k_motor22 = 169,
208 : k_motor23 = 170,
209 : k_motor24 = 171,
210 : k_motor25 = 172,
211 : k_motor26 = 173,
212 : k_motor27 = 174,
213 : k_motor28 = 175,
214 : k_motor29 = 176,
215 : k_motor30 = 177,
216 : k_motor31 = 178,
217 : k_motor32 = 179,
218 : k_cam_zoom = 180,
219 : k_lights1 = 181,
220 : k_lights2 = 182,
221 : k_video_switch = 183,
222 : k_actuator1 = 184, // Aux channels used for controlling user peripherals
223 : k_actuator2 = 185,
224 : k_actuator3 = 186,
225 : k_actuator4 = 187,
226 : k_actuator5 = 188,
227 : k_actuator6 = 189,
228 : k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one)
229 : } Function;
230 :
231 : // check if a function is valid for indexing into functions
232 1974066922 : static bool valid_function(Function fn) {
233 1974066922 : return fn >= k_none && fn < k_nr_aux_servo_functions;
234 : }
235 1351234048 : bool valid_function(void) const {
236 1351234048 : return valid_function(function);
237 : }
238 :
239 : // used to get min/max/trim limit value based on reverse
240 : enum class Limit {
241 : TRIM,
242 : MIN,
243 : MAX,
244 : ZERO_PWM
245 : };
246 :
247 : // set the output value as a pwm value
248 : void set_output_pwm(uint16_t pwm, bool force = false);
249 :
250 : // get the output value as a pwm value
251 3450955 : uint16_t get_output_pwm(void) const { return output_pwm; }
252 :
253 : // set normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
254 : void set_output_norm(float value);
255 :
256 : // set angular range of scaled output
257 : void set_angle(int16_t angle);
258 :
259 : // set range of scaled output. Low is always zero
260 : void set_range(uint16_t high);
261 :
262 : // return true if the channel is reversed
263 10000688 : bool get_reversed(void) const {
264 10000688 : return reversed != 0;
265 : }
266 :
267 : // set MIN/MAX parameters
268 2579539 : void set_output_min(uint16_t pwm) {
269 2579539 : servo_min.set(pwm);
270 2579539 : }
271 2579539 : void set_output_max(uint16_t pwm) {
272 2579539 : servo_max.set(pwm);
273 2579539 : }
274 :
275 : // get MIN/MAX/TRIM parameters
276 9806271 : uint16_t get_output_min(void) const {
277 9806271 : return servo_min;
278 : }
279 9806271 : uint16_t get_output_max(void) const {
280 9806271 : return servo_max;
281 : }
282 9989881 : uint16_t get_trim(void) const {
283 9989881 : return servo_trim;
284 : }
285 :
286 : // return true if function is for a multicopter motor
287 : static bool is_motor(Function function);
288 :
289 : // return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
290 : static bool should_e_stop(Function function);
291 :
292 : // return true if function is for a control surface
293 : static bool is_control_surface(Function function);
294 :
295 : // return the function of a channel
296 50904743 : SRV_Channel::Function get_function(void) const {
297 50904743 : return (SRV_Channel::Function)function.get();
298 : }
299 :
300 : // return the motor number of a channel, or -1 if not a motor
301 : int8_t get_motor_num(void) const;
302 :
303 : // set and save function for reversed. Used in upgrade of parameters in plane
304 : void reversed_set_and_save_ifchanged(bool r) {
305 : reversed.set_and_save_ifchanged(r?1:0);
306 : }
307 :
308 : // return true if the SERVOn_FUNCTION has been configured in
309 : // either storage or a defaults file. This is used for upgrade of
310 : // parameters in plane
311 : bool function_configured(void) const {
312 : return function.configured();
313 : }
314 :
315 : // convert a scaled value (either range or angle depending on setup) to a pwm
316 : uint16_t pwm_from_scaled_value(float scaled_value) const;
317 :
318 : // specify that small rc input changes should be ignored during passthrough
319 : // used by DO_SET_SERVO commands
320 795 : void ignore_small_rcin_changes() { ign_small_rcin_changes = true; }
321 :
322 : private:
323 : AP_Int16 servo_min;
324 : AP_Int16 servo_max;
325 : AP_Int16 servo_trim;
326 : // reversal, following convention that 1 means reversed, 0 means normal
327 : AP_Int8 reversed;
328 : AP_Enum16<Function> function;
329 :
330 : // a pending output value as PWM
331 : uint16_t output_pwm;
332 :
333 : // true for angle output type
334 : bool type_angle:1;
335 :
336 : // set_range() or set_angle() has been called
337 : bool type_setup:1;
338 :
339 : // the hal channel number
340 : uint8_t ch_num;
341 :
342 : // high point of angle or range output
343 : uint16_t high_out;
344 :
345 : // convert a 0..range_max to a pwm
346 : uint16_t pwm_from_range(float scaled_value) const;
347 :
348 : // convert a -angle_max..angle_max to a pwm
349 : uint16_t pwm_from_angle(float scaled_value) const;
350 :
351 : // convert a scaled output to a pwm value
352 : void calc_pwm(float output_scaled);
353 :
354 : // output value based on function
355 : void output_ch(void);
356 :
357 : // setup output type and range based on function
358 : void aux_servo_function_setup(void);
359 :
360 : // return PWM for a given limit value
361 : uint16_t get_limit_pwm(Limit limit) const;
362 :
363 : // get normalised output from -1 to 1, assuming 0 at mid point of servo_min/servo_max
364 : float get_output_norm(void);
365 :
366 : // a bitmask type wide enough for NUM_SERVO_CHANNELS
367 : typedef uint32_t servo_mask_t;
368 :
369 : // mask of channels where we have a output_pwm value. Cleared when a
370 : // scaled value is written.
371 : static servo_mask_t have_pwm_mask;
372 :
373 : // previous radio_in during pass-thru
374 : int16_t previous_radio_in;
375 :
376 : // specify that small rcinput changes should be ignored during passthrough
377 : // used by DO_SET_SERVO commands
378 : bool ign_small_rcin_changes;
379 :
380 : // if true we should ignore all imputs on this channel
381 : bool override_active;
382 :
383 1341455296 : void set_override(bool b) {override_active = b;};
384 : };
385 :
386 : /*
387 : class SRV_Channels
388 : */
389 : class SRV_Channels {
390 : public:
391 : friend class SRV_Channel;
392 :
393 : // constructor
394 : SRV_Channels(void);
395 :
396 : static const struct AP_Param::GroupInfo var_info[];
397 :
398 : // set the default function for a channel
399 : static void set_default_function(uint8_t chan, SRV_Channel::Function function);
400 :
401 : // set output value for a function channel as a pwm value
402 : static void set_output_pwm(SRV_Channel::Function function, uint16_t value);
403 :
404 : // set output value for a specific function channel as a pwm value
405 : static void set_output_pwm_chan(uint8_t chan, uint16_t value);
406 :
407 : // get output value for a specific channel as a pwm value
408 : static bool get_output_pwm_chan(uint8_t chan, uint16_t &value);
409 :
410 : // set output value for a specific function channel as a pwm value for specified override time in ms
411 : static void set_output_pwm_chan_timeout(uint8_t chan, uint16_t value, uint16_t timeout_ms);
412 :
413 : // set output value for a function channel as a scaled value. This
414 : // this should be followed by a call to calc_pwm() to output the pwm values
415 : static void set_output_scaled(SRV_Channel::Function function, float value);
416 :
417 : // get scaled output for the given function type.
418 : static float get_output_scaled(SRV_Channel::Function function);
419 :
420 : // get slew limited scaled output for the given function type
421 : static float get_slew_limited_output_scaled(SRV_Channel::Function function);
422 :
423 : // get pwm output for the first channel of the given function type.
424 : static bool get_output_pwm(SRV_Channel::Function function, uint16_t &value);
425 :
426 : // get normalised output (-1 to 1 with 0 at mid point of servo_min/servo_max)
427 : // Value is taken from pwm value. Returns zero on error.
428 : static float get_output_norm(SRV_Channel::Function function);
429 :
430 : // set normalised output (-1 to 1 with 0 at mid point of servo_min/servo_max) for the given function
431 : static void set_output_norm(SRV_Channel::Function function, float value);
432 :
433 : // get output channel mask for a function
434 : static uint32_t get_output_channel_mask(SRV_Channel::Function function);
435 :
436 : // limit slew rate to given limit in percent per second
437 : static void set_slew_rate(SRV_Channel::Function function, float slew_rate, uint16_t range, float dt);
438 :
439 : // update channels last_scaled_output to match value
440 : static void set_slew_last_scaled_output(SRV_Channel::Function function, float value);
441 :
442 : // call output_ch() on all channels
443 : static void output_ch_all(void);
444 :
445 : // setup output ESC scaling based on a channels MIN/MAX
446 : void set_esc_scaling_for(SRV_Channel::Function function);
447 :
448 : // return true when auto_trim enabled
449 8438671 : bool auto_trim_enabled(void) const { return auto_trim; }
450 :
451 : // adjust trim of a channel by a small increment
452 : void adjust_trim(SRV_Channel::Function function, float v);
453 :
454 : // set MIN/MAX parameters for a function
455 : static void set_output_min_max(SRV_Channel::Function function, uint16_t min_pwm, uint16_t max_pwm);
456 :
457 : // set MIN/MAX parameter defaults for a function
458 : static void set_output_min_max_defaults(SRV_Channel::Function function, uint16_t min_pwm, uint16_t max_pwm);
459 :
460 : // Save MIN/MAX/REVERSED parameters for a function
461 : static void save_output_min_max(SRV_Channel::Function function, uint16_t min_pwm, uint16_t max_pwm);
462 :
463 : // save trims
464 : void save_trim(void);
465 :
466 : // setup IO failsafe for all channels to trim
467 : static void setup_failsafe_trim_all_non_motors(void);
468 :
469 : // set output for all channels matching the given function type, allow radio_trim to center servo
470 : static void set_output_pwm_trimmed(SRV_Channel::Function function, int16_t value);
471 :
472 : // set and save the trim for a function channel to the output value
473 : static void set_trim_to_servo_out_for(SRV_Channel::Function function);
474 :
475 : // set the trim for a function channel to min of the channel honnoring reverse unless ignore_reversed is true
476 : static void set_trim_to_min_for(SRV_Channel::Function function, bool ignore_reversed = false);
477 :
478 : // set the trim for a function channel to given pwm
479 : static void set_trim_to_pwm_for(SRV_Channel::Function function, int16_t pwm);
480 :
481 : // set output to min value
482 : static void set_output_to_min(SRV_Channel::Function function);
483 :
484 : // set output to max value
485 : static void set_output_to_max(SRV_Channel::Function function);
486 :
487 : // set output to trim value
488 : static void set_output_to_trim(SRV_Channel::Function function);
489 :
490 : // copy radio_in to servo out
491 : static void copy_radio_in_out(SRV_Channel::Function function, bool do_input_output=false);
492 :
493 : // copy radio_in to servo_out by channel mask
494 : static void copy_radio_in_out_mask(uint32_t mask);
495 :
496 : // setup failsafe for an auxiliary channel function, by pwm
497 : static void set_failsafe_pwm(SRV_Channel::Function function, uint16_t pwm);
498 :
499 : // setup failsafe for an auxiliary channel function
500 : static void set_failsafe_limit(SRV_Channel::Function function, SRV_Channel::Limit limit);
501 :
502 : // set servo to a Limit
503 : static void set_output_limit(SRV_Channel::Function function, SRV_Channel::Limit limit);
504 :
505 : // return true if a function is assigned to a channel
506 : static bool function_assigned(SRV_Channel::Function function);
507 :
508 : // set a servo_out value, and angle range, then calc_pwm
509 : static void move_servo(SRV_Channel::Function function,
510 : int16_t value, int16_t angle_min, int16_t angle_max);
511 :
512 : // assign and enable auxiliary channels
513 : void enable_aux_servos(void);
514 :
515 : // enable channels by mask
516 : static void enable_by_mask(uint32_t mask);
517 :
518 : // return the current function for a channel
519 : static SRV_Channel::Function channel_function(uint8_t channel);
520 :
521 : // refresh aux servo to function mapping
522 : static void update_aux_servo_function(void);
523 :
524 : // set default channel for an auxiliary function
525 : static bool set_aux_channel_default(SRV_Channel::Function function, uint8_t channel);
526 :
527 : // find first channel that a function is assigned to
528 : static bool find_channel(SRV_Channel::Function function, uint8_t &chan);
529 :
530 : // find first channel that a function is assigned to, returning SRV_Channel object
531 : static SRV_Channel *get_channel_for(SRV_Channel::Function function);
532 :
533 : // call set_angle() on matching channels
534 : static void set_angle(SRV_Channel::Function function, uint16_t angle);
535 :
536 : // call set_range() on matching channels
537 : static void set_range(SRV_Channel::Function function, uint16_t range);
538 :
539 : // set output refresh frequency on a servo function
540 : static void set_rc_frequency(SRV_Channel::Function function, uint16_t frequency);
541 :
542 : // control pass-thru of channels
543 303 : void disable_passthrough(bool disable) {
544 303 : disabled_passthrough = disable;
545 303 : }
546 :
547 : // constrain to output min/max for function
548 : static void constrain_pwm(SRV_Channel::Function function);
549 :
550 : // calculate PWM for all channels
551 : static void calc_pwm(void);
552 :
553 : // return the ESC type for dshot commands
554 72789 : static AP_HAL::RCOutput::DshotEscType get_dshot_esc_type() { return AP_HAL::RCOutput::DshotEscType(_singleton->dshot_esc_type.get()); }
555 :
556 7 : static uint8_t get_dshot_rate() { return _singleton->dshot_rate.get(); }
557 :
558 4807 : static uint32_t get_rc_fs_mask() { return _singleton->rc_fs_mask.get(); }
559 :
560 7948520 : static SRV_Channel *srv_channel(uint8_t i) {
561 : #if NUM_SERVO_CHANNELS > 0
562 7948520 : return i<NUM_SERVO_CHANNELS?&channels[i]:nullptr;
563 : #else
564 : return nullptr;
565 : #endif
566 : }
567 :
568 : // given a zero-based motor channel, return the k_motor function for that channel
569 173128736 : static SRV_Channel::Function get_motor_function(uint8_t channel) {
570 173128736 : if (channel < 8) {
571 170749652 : return SRV_Channel::Function(SRV_Channel::k_motor1+channel);
572 : }
573 2379084 : if (channel < 12) {
574 989124 : return SRV_Channel::Function((SRV_Channel::k_motor9+(channel-8)));
575 : }
576 1389960 : return SRV_Channel::Function((SRV_Channel::k_motor13+(channel-12)));
577 : }
578 :
579 : void cork();
580 : void push();
581 :
582 : // disable PWM output to a set of channels given by a mask. This is used by the AP_BLHeli code
583 0 : static void set_disabled_channel_mask(uint32_t mask) { disabled_mask = mask; }
584 606245 : static uint32_t get_disabled_channel_mask() { return disabled_mask; }
585 :
586 : // add to mask of outputs which use digital (non-PWM) output and optionally can reverse thrust, such as DShot
587 : static void set_digital_outputs(uint32_t dig_mask, uint32_t rev_mask);
588 :
589 : // return true if all of the outputs in mask are digital
590 27173 : static bool have_digital_outputs(uint32_t mask) { return mask != 0 && (mask & digital_mask) == mask; }
591 :
592 : // return true if any of the outputs are digital
593 0 : static bool have_digital_outputs() { return digital_mask != 0; }
594 :
595 : // Set E - stop
596 : static void set_emergency_stop(bool state);
597 :
598 : // get E - stop
599 36971509 : static bool get_emergency_stop() { return emergency_stop;}
600 :
601 : // singleton for Lua
602 50367196 : static SRV_Channels *get_singleton(void) {
603 50367196 : return _singleton;
604 : }
605 :
606 : static void zero_rc_outputs();
607 :
608 : // initialize before any call to push
609 : void init(uint32_t motor_mask = 0, AP_HAL::RCOutput::output_mode mode = AP_HAL::RCOutput::MODE_PWM_NONE);
610 :
611 : // return true if a channel is set to type GPIO
612 : static bool is_GPIO(uint8_t channel);
613 :
614 : // return true if a channel is set to type alarm
615 : static bool is_alarm(uint8_t channel) {
616 : return channel_function(channel) == SRV_Channel::k_alarm;
617 : }
618 :
619 : // return true if a channel is set to type alarm inverted
620 : static bool is_alarm_inverted(uint8_t channel) {
621 : return channel_function(channel) == SRV_Channel::k_alarm_inverted;
622 : }
623 :
624 : // return true if 32 channels are enabled
625 0 : static bool have_32_channels() {
626 : #if NUM_SERVO_CHANNELS >= 17
627 0 : return _singleton->enable_32_channels.get() > 0;
628 : #else
629 : return false;
630 : #endif
631 : }
632 :
633 : private:
634 :
635 : static bool disabled_passthrough;
636 :
637 : SRV_Channel::servo_mask_t trimmed_mask;
638 :
639 : static Bitmask<SRV_Channel::k_nr_aux_servo_functions> function_mask;
640 : static bool initialised;
641 :
642 : // this static arrangement is to avoid having static objects in AP_Param tables
643 : static SRV_Channel *channels;
644 : static SRV_Channels *_singleton;
645 :
646 : #if AP_VOLZ_ENABLED
647 : // support for Volz protocol
648 : AP_Volz_Protocol volz;
649 : #endif
650 :
651 : #if AP_SBUSOUTPUT_ENABLED
652 : // support for SBUS protocol
653 : AP_SBusOut sbus;
654 : #endif
655 :
656 : #if AP_ROBOTISSERVO_ENABLED
657 : // support for Robotis servo protocol
658 : AP_RobotisServo robotis;
659 : #endif
660 :
661 : #if HAL_SUPPORT_RCOUT_SERIAL
662 : // support for BLHeli protocol
663 : AP_BLHeli blheli;
664 : #endif
665 :
666 : #if AP_FETTEC_ONEWIRE_ENABLED
667 : AP_FETtecOneWire fetteconwire;
668 : #endif // AP_FETTEC_ONEWIRE_ENABLED
669 :
670 : // mask of disabled channels
671 : static uint32_t disabled_mask;
672 :
673 : // mask of outputs which use a digital output protocol, not
674 : // PWM (eg. DShot)
675 : static uint32_t digital_mask;
676 :
677 : // mask of outputs which are digitally reversible (eg. DShot-3D)
678 : static uint32_t reversible_mask;
679 :
680 : // mask of channels with invalid funtions, eg GPIO
681 : static uint32_t invalid_mask;
682 :
683 : SRV_Channel obj_channels[NUM_SERVO_CHANNELS];
684 :
685 : // override loop counter
686 : static uint16_t override_counter[NUM_SERVO_CHANNELS];
687 :
688 : static struct srv_function {
689 : // mask of what channels this applies to
690 : SRV_Channel::servo_mask_t channel_mask;
691 :
692 : // scaled output for this function
693 : float output_scaled;
694 : } functions[SRV_Channel::k_nr_aux_servo_functions];
695 :
696 : AP_Int8 auto_trim;
697 : AP_Int16 default_rate;
698 : AP_Int8 dshot_rate;
699 : AP_Int8 dshot_esc_type;
700 : AP_Int32 gpio_mask;
701 : AP_Int32 rc_fs_mask;
702 : #if NUM_SERVO_CHANNELS >= 17
703 : AP_Int8 enable_32_channels;
704 : #endif
705 :
706 : // return true if passthrough is disabled
707 11200 : static bool passthrough_disabled(void) {
708 11200 : return disabled_passthrough;
709 : }
710 :
711 : static bool emergency_stop;
712 :
713 : // linked list for slew rate handling
714 : struct slew_list {
715 3455 : slew_list(SRV_Channel::Function _func) : func(_func) {};
716 : const SRV_Channel::Function func;
717 : float last_scaled_output;
718 : float max_change;
719 : slew_list * next;
720 : };
721 : static slew_list *_slew;
722 :
723 : // semaphore for multi-thread use of override_counter array
724 : HAL_Semaphore override_counter_sem;
725 : };
726 :
727 : namespace AP {
728 : SRV_Channels &srv();
729 : };
|
