Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : * RC_Channel.cpp - class for one RC channel input
18 : */
19 :
20 : #include "RC_Channel_config.h"
21 :
22 : #if AP_RC_CHANNEL_ENABLED
23 :
24 : #include <stdlib.h>
25 : #include <cmath>
26 :
27 : #include <AP_HAL/AP_HAL.h>
28 : extern const AP_HAL::HAL& hal;
29 :
30 : #include <AP_Math/AP_Math.h>
31 :
32 : #include "RC_Channel.h"
33 : #include <GCS_MAVLink/GCS.h>
34 :
35 : #include <AC_Avoidance/AC_Avoid.h>
36 : #include <AC_Sprayer/AC_Sprayer.h>
37 : #include <AP_Camera/AP_Camera.h>
38 : #include <AP_Camera/AP_RunCam.h>
39 : #include <AP_Compass/AP_Compass.h>
40 : #include <AP_Generator/AP_Generator.h>
41 : #include <AP_Gripper/AP_Gripper.h>
42 : #include <AP_GyroFFT/AP_GyroFFT.h>
43 : #include <AP_ADSB/AP_ADSB.h>
44 : #include <AP_BoardConfig/AP_BoardConfig.h>
45 : #include <AP_BattMonitor/AP_BattMonitor.h>
46 : #include <AP_LandingGear/AP_LandingGear.h>
47 : #include <AP_Logger/AP_Logger.h>
48 : #include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
49 : #include <SRV_Channel/SRV_Channel.h>
50 : #include <AP_Arming/AP_Arming.h>
51 : #include <AP_Avoidance/AP_Avoidance.h>
52 : #include <AP_GPS/AP_GPS.h>
53 : #include <AC_Fence/AC_Fence.h>
54 : #include <AP_OpticalFlow/AP_OpticalFlow.h>
55 : #include <AP_VisualOdom/AP_VisualOdom.h>
56 : #include <AP_AHRS/AP_AHRS.h>
57 : #include <AP_Mount/AP_Mount.h>
58 : #include <AP_Notify/AP_Notify.h>
59 : #include <AP_VideoTX/AP_VideoTX.h>
60 : #include <AP_Torqeedo/AP_Torqeedo.h>
61 : #include <AP_Vehicle/AP_Vehicle_Type.h>
62 : #include <AP_Parachute/AP_Parachute_config.h>
63 : #include <AP_Scripting/AP_Scripting.h>
64 : #define SWITCH_DEBOUNCE_TIME_MS 200
65 :
66 : const AP_Param::GroupInfo RC_Channel::var_info[] = {
67 : // @Param: MIN
68 : // @DisplayName: RC min PWM
69 : // @Description: RC minimum PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
70 : // @Units: PWM
71 : // @Range: 800 2200
72 : // @Increment: 1
73 : // @User: Advanced
74 : AP_GROUPINFO("MIN", 1, RC_Channel, radio_min, 1100),
75 :
76 : // @Param: TRIM
77 : // @DisplayName: RC trim PWM
78 : // @Description: RC trim (neutral) PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
79 : // @Units: PWM
80 : // @Range: 800 2200
81 : // @Increment: 1
82 : // @User: Advanced
83 : AP_GROUPINFO("TRIM", 2, RC_Channel, radio_trim, 1500),
84 :
85 : // @Param: MAX
86 : // @DisplayName: RC max PWM
87 : // @Description: RC maximum PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
88 : // @Units: PWM
89 : // @Range: 800 2200
90 : // @Increment: 1
91 : // @User: Advanced
92 : AP_GROUPINFO("MAX", 3, RC_Channel, radio_max, 1900),
93 :
94 : // @Param: REVERSED
95 : // @DisplayName: RC reversed
96 : // @Description: Reverse channel input. Set to 0 for normal operation. Set to 1 to reverse this input channel.
97 : // @Values: 0:Normal,1:Reversed
98 : // @User: Advanced
99 : AP_GROUPINFO("REVERSED", 4, RC_Channel, reversed, 0),
100 :
101 : // @Param: DZ
102 : // @DisplayName: RC dead-zone
103 : // @Description: PWM dead zone in microseconds around trim or bottom
104 : // @Units: PWM
105 : // @Range: 0 200
106 : // @User: Advanced
107 : AP_GROUPINFO("DZ", 5, RC_Channel, dead_zone, 0),
108 :
109 : // @Param: OPTION
110 : // @DisplayName: RC input option
111 : // @Description: Function assigned to this RC channel
112 : // @SortValues: AlphabeticalZeroAtTop
113 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 0:Do Nothing
114 : // @Values{Copter}: 2:FLIP Mode
115 : // @Values{Copter}: 3:Simple Mode
116 : // @Values{Copter, Rover, Plane}: 4:RTL
117 : // @Values{Copter}: 5:Save Trim
118 : // @Values{Rover}: 5:Save Trim (4.1 and lower)
119 : // @Values{Copter, Rover}: 7:Save WP
120 : // @Values{Copter, Rover, Plane, Sub}: 9:Camera Trigger
121 : // @Values{Copter}: 10:RangeFinder Enable
122 : // @Values{Copter, Rover, Plane, Sub}: 11:Fence Enable
123 : // @Values{Copter}: 13:Super Simple Mode
124 : // @Values{Copter}: 14:Acro Trainer
125 : // @Values{Copter}: 15:Sprayer Enable
126 : // @Values{Copter, Rover, Plane}: 16:AUTO Mode
127 : // @Values{Copter}: 17:AUTOTUNE Mode
128 : // @Values{Copter, Blimp}: 18:LAND Mode
129 : // @Values{Copter, Rover}: 19:Gripper
130 : // @Values{Copter}: 21:Parachute Enable
131 : // @Values{Copter, Plane}: 22:Parachute Release
132 : // @Values{Copter}: 23:Parachute 3pos
133 : // @Values{Copter, Rover, Plane, Sub}: 24:Auto Mission Reset
134 : // @Values{Copter}: 25:AttCon Feed Forward
135 : // @Values{Copter}: 26:AttCon Accel Limits
136 : // @Values{Copter, Rover, Plane, Sub}: 27:Retract Mount1
137 : // @Values{Copter, Rover, Plane, Sub}: 28:Relay1 On/Off
138 : // @Values{Copter, Plane}: 29:Landing Gear
139 : // @Values{Copter}: 30:Lost Copter Sound
140 : // @Values{Rover}: 30:Lost Rover Sound
141 : // @Values{Plane}: 30:Lost Plane Sound
142 : // @Values{Copter, Rover, Plane, Sub}: 31:Motor Emergency Stop
143 : // @Values{Copter}: 32:Motor Interlock
144 : // @Values{Copter}: 33:BRAKE Mode
145 : // @Values{Copter, Rover, Plane, Sub}: 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off
146 : // @Values{Copter}: 37:THROW Mode
147 : // @Values{Copter, Plane}: 38:ADSB Avoidance Enable
148 : // @Values{Copter}: 39:PrecLoiter Enable
149 : // @Values{Copter, Rover}: 40:Proximity Avoidance Enable
150 : // @Values{Copter, Rover, Plane}: 41:ArmDisarm (4.1 and lower)
151 : // @Values{Copter, Rover}: 42:SMARTRTL Mode
152 : // @Values{Copter, Plane}: 43:InvertedFlight Enable
153 : // @Values{Copter}: 44:Winch Enable, 45:Winch Control
154 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 46:RC Override Enable
155 : // @Values{Copter}: 47:User Function 1, 48:User Function 2, 49:User Function 3
156 : // @Values{Rover}: 50:LearnCruise Speed
157 : // @Values{Rover, Plane}: 51:MANUAL Mode
158 : // @Values{Copter, Rover, Plane}: 52:ACRO Mode
159 : // @Values{Rover}: 53:STEERING Mode
160 : // @Values{Rover}: 54:HOLD Mode
161 : // @Values{Copter, Rover, Plane}: 55:GUIDED Mode
162 : // @Values{Copter, Rover, Plane}: 56:LOITER Mode
163 : // @Values{Copter, Rover}: 57:FOLLOW Mode
164 : // @Values{Copter, Rover, Plane, Sub}: 58:Clear Waypoints
165 : // @Values{Rover}: 59:Simple Mode
166 : // @Values{Copter}: 60:ZigZag Mode
167 : // @Values{Copter}: 61:ZigZag SaveWP
168 : // @Values{Copter, Rover, Plane, Sub}: 62:Compass Learn
169 : // @Values{Rover}: 63:Sailboat Tack
170 : // @Values{Plane}: 64:Reverse Throttle
171 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 65:GPS Disable
172 : // @Values{Copter, Rover, Plane, Sub}: 66:Relay5 On/Off, 67:Relay6 On/Off
173 : // @Values{Copter}: 68:STABILIZE Mode
174 : // @Values{Copter}: 69:POSHOLD Mode
175 : // @Values{Copter}: 70:ALTHOLD Mode
176 : // @Values{Copter}: 71:FLOWHOLD Mode
177 : // @Values{Copter,Rover,Plane}: 72:CIRCLE Mode
178 : // @Values{Copter}: 73:DRIFT Mode
179 : // @Values{Rover}: 74:Sailboat motoring 3pos
180 : // @Values{Copter}: 75:SurfaceTrackingUpDown
181 : // @Values{Copter}: 76:STANDBY Mode
182 : // @Values{Plane}: 77:TAKEOFF Mode
183 : // @Values{Copter, Rover, Plane, Sub}: 78:RunCam Control
184 : // @Values{Copter, Rover, Plane, Sub}: 79:RunCam OSD Control
185 : // @Values{Copter}: 80:VisOdom Align
186 : // @Values{Rover}: 80:VisoOdom Align
187 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 81:Disarm
188 : // @Values{Plane}: 82:QAssist 3pos
189 : // @Values{Copter}: 83:ZigZag Auto
190 : // @Values{Copter, Plane}: 84:AirMode
191 : // @Values{Copter, Plane}: 85:Generator
192 : // @Values{Plane}: 86:Non Auto Terrain Follow Disable
193 : // @Values{Plane}: 87:Crow Select
194 : // @Values{Plane}: 88:Soaring Enable
195 : // @Values{Plane}: 89:Landing Flare
196 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 90:EKF Source Set
197 : // @Values{Plane}: 91:Airspeed Ratio Calibration
198 : // @Values{Plane}: 92:FBWA Mode
199 : // @Values{Copter, Rover, Plane, Sub}: 94:VTX Power
200 : // @Values{Plane}: 95:FBWA taildragger takeoff mode
201 : // @Values{Plane}: 96:Trigger re-reading of mode switch
202 : // @Values{Rover}: 97:Windvane home heading direction offset
203 : // @Values{Plane}: 98:TRAINING Mode
204 : // @Values{Copter}: 99:AUTO RTL
205 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 100:KillIMU1, 101:KillIMU2
206 : // @Values{Copter, Rover, Plane, Sub}: 102:Camera Mode Toggle
207 : // @Values{Copter, Rover, Plane, Blimp, Sub, Tracker}: 103: EKF lane switch attempt
208 : // @Values{Copter, Rover, Plane, Blimp, Sub, Tracker}: 104: EKF yaw reset
209 : // @Values{Copter, Rover, Plane, Sub}: 105:GPS Disable Yaw
210 : // @Values{Rover, Plane}: 106:Disable Airspeed Use
211 : // @Values{Plane}: 107:Enable FW Autotune
212 : // @Values{Plane}: 108:QRTL Mode
213 : // @Values{Copter}: 109:use Custom Controller
214 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 110:KillIMU3
215 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 111:Loweheiser starter
216 : // @Values{Copter,Plane,Rover,Blimp,Sub,Tracker}: 112:SwitchExternalAHRS
217 : // @Values{Copter, Rover, Plane, Sub}: 113:Retract Mount2
218 : // @Values{Plane}: 150:CRUISE Mode
219 : // @Values{Copter}: 151:TURTLE Mode
220 : // @Values{Copter}: 152:SIMPLE heading reset
221 : // @Values{Copter, Rover, Plane, Sub}: 153:ArmDisarm (4.2 and higher)
222 : // @Values{Blimp}: 153:ArmDisarm
223 : // @Values{Copter}: 154:ArmDisarm with AirMode (4.2 and higher)
224 : // @Values{Plane}: 154:ArmDisarm with Quadplane AirMode (4.2 and higher)
225 : // @Values{Rover}: 155:Set steering trim to current servo and RC
226 : // @Values{Plane}: 155:Set roll pitch and yaw trim to current servo and RC
227 : // @Values{Rover}: 156:Torqeedo Clear Err
228 : // @Values{Plane}: 157:Force FS Action to FBWA
229 : // @Values{Copter, Plane}: 158:Optflow Calibration
230 : // @Values{Copter}: 159:Force IS_Flying
231 : // @Values{Plane}: 160:Weathervane Enable
232 : // @Values{Copter}: 161:Turbine Start(heli)
233 : // @Values{Copter, Rover, Plane}: 162:FFT Tune
234 : // @Values{Copter, Rover, Plane, Sub}: 163:Mount Yaw Lock
235 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 164:Pause Stream Logging
236 : // @Values{Copter, Rover, Plane, Sub}: 165:Arm/Emergency Motor Stop
237 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 166:Camera Record Video, 167:Camera Zoom, 168:Camera Manual Focus, 169:Camera Auto Focus
238 : // @Values{Plane}: 170:QSTABILIZE Mode
239 : // @Values{Copter, Rover, Plane, Blimp}: 171:Calibrate Compasses
240 : // @Values{Copter, Rover, Plane, Blimp}: 172:Battery MPPT Enable
241 : // @Values{Plane}: 173:Plane AUTO Mode Landing Abort
242 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 174:Camera Image Tracking
243 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 175:Camera Lens
244 : // @Values{Plane}: 176:Quadplane Fwd Throttle Override enable
245 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 177:Mount LRF enable
246 : // @Values{Copter}: 178:FlightMode Pause/Resume
247 : // @Values{Plane}: 179:ICEngine start / stop
248 : // @Values{Copter, Plane}: 180:Test autotuned gains after tune is complete
249 : // @Values{Plane}: 181: QuickTune
250 : // @Values{Copter}: 182: AHRS AutoTrim
251 : // @Values{Plane}: 183: AUTOLAND mode
252 : // @Values{Plane}: 184: System ID Chirp
253 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 185:Mount Roll/Pitch Lock
254 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 186:Mount POI Lock
255 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 187:EKF Reset
256 : // @Values{Rover}: 201:Roll
257 : // @Values{Rover}: 202:Pitch
258 : // @Values{Rover}: 207:MainSail
259 : // @Values{Rover, Plane}: 208:Flap
260 : // @Values{Plane}: 209:VTOL Forward Throttle
261 : // @Values{Plane}: 210:Airbrakes
262 : // @Values{Rover}: 211:Walking Height
263 : // @Values{Copter, Rover, Plane, Sub}: 212:Mount1 Roll, 213:Mount1 Pitch, 214:Mount1 Yaw, 215:Mount2 Roll, 216:Mount2 Pitch, 217:Mount2 Yaw
264 : // @Values{Copter, Rover, Plane, Blimp, Sub}: 218:Loweheiser throttle
265 : // @Values{Copter}: 219:Transmitter Tuning
266 : // @Values{All-Vehicles}: 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8, 308:Scripting9, 309:Scripting10, 310:Scripting11, 311:Scripting12, 312:Scripting13, 313:Scripting14, 314:Scripting15, 315:Scripting16
267 : // @Values{All-Vehicles}: 316:Stop-Restart Scripting
268 : // @User: Standard
269 : AP_GROUPINFO("OPTION", 6, RC_Channel, option, 0),
270 :
271 : AP_GROUPEND
272 : };
273 :
274 :
275 : // constructor
276 35697 : RC_Channel::RC_Channel(void)
277 : {
278 35697 : AP_Param::setup_object_defaults(this, var_info);
279 35697 : }
280 :
281 56138 : void RC_Channel::set_range(uint16_t high)
282 : {
283 56138 : type_in = ControlType::RANGE;
284 56138 : high_in = high;
285 56138 : }
286 :
287 206998 : void RC_Channel::set_angle(uint16_t angle)
288 : {
289 206998 : type_in = ControlType::ANGLE;
290 206998 : high_in = angle;
291 206998 : }
292 :
293 8121 : void RC_Channel::set_default_dead_zone(int16_t dzone)
294 : {
295 8121 : dead_zone.set_default(abs(dzone));
296 8121 : }
297 :
298 40421032 : bool RC_Channel::get_reverse(void) const
299 : {
300 40421032 : return bool(reversed.get());
301 : }
302 :
303 : // read input from hal.rcin or overrides
304 119958672 : bool RC_Channel::update(void)
305 : {
306 119958672 : raw_radio_in = hal.rcin->read(ch_in);
307 119958672 : if (has_override() && !rc().option_is_enabled(RC_Channels::Option::IGNORE_OVERRIDES)) {
308 55180 : radio_in = override_value;
309 119903492 : } else if (rc().has_had_rc_receiver() && !rc().option_is_enabled(RC_Channels::Option::IGNORE_RECEIVER)) {
310 119899256 : radio_in = raw_radio_in;
311 : } else {
312 4236 : return false;
313 : }
314 :
315 119954436 : if (type_in == ControlType::RANGE) {
316 6809620 : control_in = pwm_to_range();
317 : } else {
318 : // ControlType::ANGLE
319 113144816 : control_in = pwm_to_angle();
320 : }
321 :
322 119954436 : return true;
323 : }
324 :
325 : /*
326 : return the center stick position expressed as a control_in value
327 : used for thr_mid in copter
328 : */
329 14958411 : int16_t RC_Channel::get_control_mid() const
330 : {
331 14958411 : if (type_in == ControlType::RANGE) {
332 14958411 : int16_t r_in = (radio_min.get() + radio_max.get())/2;
333 :
334 14958411 : int16_t radio_trim_low = radio_min + dead_zone;
335 :
336 14958411 : return (((int32_t)(high_in) * (int32_t)(r_in - radio_trim_low)) / (int32_t)(radio_max - radio_trim_low));
337 : } else {
338 0 : return 0;
339 : }
340 : }
341 :
342 : /*
343 : return an "angle in centidegrees" (normally -4500 to 4500) from
344 : the current radio_in value using the specified dead_zone
345 : */
346 116127266 : float RC_Channel::pwm_to_angle_dz_trim(uint16_t _dead_zone, uint16_t _trim) const
347 : {
348 116127266 : int16_t radio_trim_high = _trim + _dead_zone;
349 116127266 : int16_t radio_trim_low = _trim - _dead_zone;
350 :
351 116127266 : float reverse_mul = (reversed?-1:1);
352 :
353 : // don't allow out of range values
354 116127266 : int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get());
355 :
356 116127266 : if (r_in > radio_trim_high && radio_max != radio_trim_high) {
357 7350829 : return reverse_mul * ((float)high_in * (float)(r_in - radio_trim_high)) / (float)(radio_max - radio_trim_high);
358 108776437 : } else if (r_in < radio_trim_low && radio_trim_low != radio_min) {
359 937349 : return reverse_mul * ((float)high_in * (float)(r_in - radio_trim_low)) / (float)(radio_trim_low - radio_min);
360 : } else {
361 107839088 : return 0;
362 : }
363 : }
364 :
365 : /*
366 : return an "angle in centidegrees" (normally -4500 to 4500) from
367 : the current radio_in value using the specified dead_zone
368 : */
369 115172981 : float RC_Channel::pwm_to_angle_dz(uint16_t _dead_zone) const
370 : {
371 115172981 : return pwm_to_angle_dz_trim(_dead_zone, radio_trim);
372 : }
373 :
374 : /*
375 : return an "angle in centidegrees" (normally -4500 to 4500) from
376 : the current radio_in value
377 : */
378 113144816 : float RC_Channel::pwm_to_angle() const
379 : {
380 113144816 : return pwm_to_angle_dz(dead_zone);
381 : }
382 :
383 :
384 : /*
385 : convert a pulse width modulation value to a value in the configured
386 : range, using the specified deadzone
387 : */
388 12327142 : float RC_Channel::pwm_to_range_dz(uint16_t _dead_zone) const
389 : {
390 12327142 : int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get());
391 :
392 12327142 : if (reversed) {
393 0 : r_in = radio_max.get() - (r_in - radio_min.get());
394 : }
395 :
396 12327142 : int16_t radio_trim_low = radio_min + _dead_zone;
397 :
398 12327142 : if (r_in > radio_trim_low) {
399 3934259 : return (((float)(high_in) * (float)(r_in - radio_trim_low)) / (float)(radio_max - radio_trim_low));
400 : }
401 8392883 : return 0;
402 : }
403 :
404 : /*
405 : convert a pulse width modulation value to a value in the configured
406 : range
407 : */
408 6809620 : float RC_Channel::pwm_to_range() const
409 : {
410 6809620 : return pwm_to_range_dz(dead_zone);
411 : }
412 :
413 :
414 7545687 : float RC_Channel::get_control_in_zero_dz(void) const
415 : {
416 7545687 : if (type_in == ControlType::RANGE) {
417 5517522 : return pwm_to_range_dz(0);
418 : }
419 2028165 : return pwm_to_angle_dz(0);
420 : }
421 :
422 : // ------------------------------------------
423 :
424 15612637 : float RC_Channel::norm_input() const
425 : {
426 : float ret;
427 15612637 : int16_t reverse_mul = (reversed?-1:1);
428 15612637 : if (radio_in < radio_trim) {
429 245731 : if (radio_min >= radio_trim) {
430 0 : return 0.0f;
431 : }
432 245731 : ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min);
433 : } else {
434 15366906 : if (radio_max <= radio_trim) {
435 0 : return 0.0f;
436 : }
437 15366906 : ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim);
438 : }
439 15612637 : return constrain_float(ret, -1.0f, 1.0f);
440 : }
441 :
442 75876353 : float RC_Channel::norm_input_dz() const
443 : {
444 75876353 : int16_t dz_min = radio_trim - dead_zone;
445 75876353 : int16_t dz_max = radio_trim + dead_zone;
446 : float ret;
447 75876353 : int16_t reverse_mul = (reversed?-1:1);
448 75876353 : if (radio_in < dz_min && dz_min > radio_min) {
449 819444 : ret = reverse_mul * (float)(radio_in - dz_min) / (float)(dz_min - radio_min);
450 75056909 : } else if (radio_in > dz_max && radio_max > dz_max) {
451 736847 : ret = reverse_mul * (float)(radio_in - dz_max) / (float)(radio_max - dz_max);
452 : } else {
453 74320062 : ret = 0;
454 : }
455 75876353 : return constrain_float(ret, -1.0f, 1.0f);
456 : }
457 :
458 : // return a normalised input for a channel, in range -1 to 1,
459 : // ignores trim and deadzone
460 10112 : float RC_Channel::norm_input_ignore_trim() const
461 : {
462 : // sanity check min and max to avoid divide by zero
463 10112 : if (radio_max <= radio_min) {
464 0 : return 0.0f;
465 : }
466 10112 : const float ret = (reversed ? -2.0f : 2.0f) * (((float)(radio_in - radio_min) / (float)(radio_max - radio_min)) - 0.5f);
467 10112 : return constrain_float(ret, -1.0f, 1.0f);
468 : }
469 :
470 :
471 1386 : bool RC_Channel::norm_input_ignore_trim(float &norm_in) const
472 : {
473 1386 : if (!rc().has_valid_input()) {
474 0 : return false;
475 : }
476 1386 : if (radio_in == 0) {
477 0 : return false;
478 : }
479 1386 : if (radio_max <= radio_min) {
480 : // sanity check min and max to avoid divide by zero
481 0 : return false;
482 : }
483 1386 : norm_in = norm_input_ignore_trim();
484 1386 : return true;
485 : }
486 :
487 : /*
488 : get percentage input from 0 to 100. This ignores the trim value.
489 : */
490 13517 : uint8_t RC_Channel::percent_input() const
491 : {
492 13517 : if (radio_in <= radio_min) {
493 13242 : return reversed?100:0;
494 : }
495 275 : if (radio_in >= radio_max) {
496 70 : return reversed?0:100;
497 : }
498 205 : uint8_t ret = 100.0f * (radio_in - radio_min) / (float)(radio_max - radio_min);
499 205 : if (reversed) {
500 0 : ret = 100 - ret;
501 : }
502 205 : return ret;
503 : }
504 :
505 : /*
506 : return true if input is within deadzone of trim
507 : */
508 0 : bool RC_Channel::in_trim_dz() const
509 : {
510 0 : return is_bounded_int32(radio_in, radio_trim - dead_zone, radio_trim + dead_zone);
511 : }
512 :
513 : /*
514 : return true if raw input is within deadzone of trim
515 : */
516 1041 : bool RC_Channel::in_raw_trim_dz() const
517 : {
518 1041 : return is_bounded_int32(raw_radio_in, radio_trim - dead_zone, radio_trim + dead_zone);
519 : }
520 :
521 : /*
522 : return trues if input is within deadzone of min
523 : */
524 0 : bool RC_Channel::in_min_dz() const
525 : {
526 0 : return radio_in < radio_min + dead_zone;
527 : }
528 :
529 4110 : void RC_Channel::set_override(const uint16_t v, const uint32_t timestamp_ms)
530 : {
531 4110 : if (!rc().gcs_overrides_enabled()) {
532 374 : return;
533 : }
534 :
535 : // the channel that controls override-enable must always reflect the
536 : // physical RC switch; accepting a GCS override here would let the GCS
537 : // disable its own overrides and create an oscillation
538 3736 : if ((AUX_FUNC)option.get() == AUX_FUNC::RC_OVERRIDE_ENABLE) {
539 151 : return;
540 : }
541 :
542 : // store throttle pwm at the start of an override session
543 3585 : if (!rc().has_active_overrides()) {
544 116 : rc().set_override_start_throttle(rc().get_throttle_channel().get_raw_radio_in());
545 : }
546 :
547 3585 : last_override_time = timestamp_ms != 0 ? timestamp_ms : AP_HAL::millis();
548 3585 : override_value = v;
549 3585 : rc().new_override_received();
550 : }
551 :
552 967824 : void RC_Channel::clear_override()
553 : {
554 967824 : last_override_time = 0;
555 967824 : override_value = 0;
556 : // only forget the throttle override-start pwm once no overrides remain
557 967824 : if (!rc().has_active_overrides()) {
558 967816 : rc().set_override_start_throttle(-1);
559 : }
560 967824 : }
561 :
562 170704413 : bool RC_Channel::has_override() const
563 : {
564 170704413 : if (override_value == 0) {
565 168013394 : return false;
566 : }
567 :
568 : uint32_t override_timeout_ms;
569 2691019 : if (!rc().get_override_timeout_ms(override_timeout_ms)) {
570 : // timeouts are disabled
571 3981 : return true;
572 : }
573 :
574 2687038 : if (override_timeout_ms == 0) {
575 : // overrides are explicitly disabled by a zero value
576 112 : return false;
577 : }
578 :
579 2686926 : return (AP_HAL::millis() - last_override_time < override_timeout_ms);
580 : }
581 :
582 : /*
583 : perform stick mixing on one channel
584 : This type of stick mixing reduces the influence of the auto
585 : controller as it increases the influence of the users stick input,
586 : allowing the user full deflection if needed
587 : */
588 126941 : float RC_Channel::stick_mixing(const float servo_in)
589 : {
590 126941 : float ch_inf = (float)(radio_in - radio_trim);
591 126941 : ch_inf = fabsf(ch_inf);
592 126941 : ch_inf = MIN(ch_inf, 400.0f);
593 126941 : ch_inf = ((400.0f - ch_inf) / 400.0f);
594 :
595 126941 : float servo_out = servo_in;
596 126941 : servo_out *= ch_inf;
597 126941 : servo_out += control_in;
598 :
599 126941 : return servo_out;
600 : }
601 :
602 : //
603 : // support for auxiliary switches:
604 : //
605 :
606 2707 : void RC_Channel::reset_mode_switch()
607 : {
608 2707 : switch_state.current_position = -1;
609 2707 : switch_state.debounce_position = -1;
610 2707 : read_mode_switch();
611 2707 : }
612 :
613 : // read a 6 position switch
614 30421640 : bool RC_Channel::read_6pos_switch(int8_t& position)
615 : {
616 : // calculate position of 6 pos switch
617 30421640 : const uint16_t pulsewidth = get_radio_in();
618 30421640 : if (pulsewidth <= RC_MIN_LIMIT_PWM || pulsewidth >= RC_MAX_LIMIT_PWM) {
619 30570 : return false; // This is an error condition
620 : }
621 :
622 30391070 : if (pulsewidth < 1231) {
623 49567 : position = 0;
624 30341503 : } else if (pulsewidth < 1361) {
625 22256 : position = 1;
626 30319247 : } else if (pulsewidth < 1491) {
627 1205 : position = 2;
628 30318042 : } else if (pulsewidth < 1621) {
629 153324 : position = 3;
630 30164718 : } else if (pulsewidth < 1750) {
631 1269 : position = 4;
632 : } else {
633 30163449 : position = 5;
634 : }
635 :
636 30391070 : if (!debounce_completed(position)) {
637 30389027 : return false;
638 : }
639 :
640 2043 : return true;
641 : }
642 :
643 30421640 : void RC_Channel::read_mode_switch()
644 : {
645 : int8_t position;
646 30421640 : if (read_6pos_switch(position)) {
647 : // set flight mode and simple mode setting
648 2043 : mode_switch_changed(modeswitch_pos_t(position));
649 : }
650 30421640 : }
651 :
652 31398111 : bool RC_Channel::debounce_completed(int8_t position)
653 : {
654 : // switch change not detected
655 31398111 : if (switch_state.current_position == position) {
656 : // reset debouncing
657 31305823 : switch_state.debounce_position = position;
658 : } else {
659 : // switch change detected
660 92288 : const uint32_t tnow_ms = AP_HAL::millis();
661 :
662 : // position not established yet
663 92288 : if (switch_state.debounce_position != position) {
664 4025 : switch_state.debounce_position = position;
665 4025 : switch_state.last_edge_time_ms = tnow_ms;
666 88263 : } else if (tnow_ms - switch_state.last_edge_time_ms >= SWITCH_DEBOUNCE_TIME_MS) {
667 : // position estabilished; debounce completed
668 3999 : switch_state.current_position = position;
669 3999 : return true;
670 : }
671 : }
672 :
673 31394112 : return false;
674 : }
675 :
676 : //
677 : // support for auxiliary switches:
678 : //
679 :
680 : // init_aux_switch_function - initialize aux functions
681 32425 : void RC_Channel::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag)
682 : {
683 : // init channel options
684 32425 : switch (ch_option) {
685 : // the following functions do not need to be initialised:
686 : #if AP_ARMING_ENABLED
687 32406 : case AUX_FUNC::ARMDISARM:
688 : case AUX_FUNC::ARMDISARM_AIRMODE:
689 : #endif
690 : #if AP_BATTERY_ENABLED
691 : case AUX_FUNC::BATTERY_MPPT_ENABLE:
692 : #endif
693 : #if AP_CAMERA_ENABLED
694 : case AUX_FUNC::CAMERA_TRIGGER:
695 : #endif
696 : #if AP_MISSION_ENABLED
697 : case AUX_FUNC::CLEAR_WP:
698 : #endif
699 : case AUX_FUNC::COMPASS_LEARN:
700 : #if AP_ARMING_ENABLED
701 : case AUX_FUNC::DISARM:
702 : #endif
703 : case AUX_FUNC::DO_NOTHING:
704 : #if AP_GRIPPER_ENABLED
705 : case AUX_FUNC::GRIPPER:
706 : #endif
707 : #if AP_LANDINGGEAR_ENABLED
708 : case AUX_FUNC::LANDING_GEAR:
709 : #endif
710 : case AUX_FUNC::LOST_VEHICLE_SOUND:
711 : #if AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED
712 : case AUX_FUNC::RELAY:
713 : case AUX_FUNC::RELAY2:
714 : case AUX_FUNC::RELAY3:
715 : case AUX_FUNC::RELAY4:
716 : case AUX_FUNC::RELAY5:
717 : case AUX_FUNC::RELAY6:
718 : #endif
719 : #if HAL_VISUALODOM_ENABLED
720 : case AUX_FUNC::VISODOM_ALIGN:
721 : #endif
722 : #if AP_AHRS_ENABLED
723 : case AUX_FUNC::EKF_LANE_SWITCH:
724 : case AUX_FUNC::EKF_YAW_RESET:
725 : #endif
726 : #if AP_AHRS_EKF_RESET_ENABLED
727 : case AUX_FUNC::EKF_RESET:
728 : #endif
729 : #if HAL_GENERATOR_ENABLED
730 : case AUX_FUNC::GENERATOR: // don't turn generator on or off initially
731 : #endif
732 : #if AP_AHRS_ENABLED
733 : case AUX_FUNC::EKF_SOURCE_SET:
734 : #endif
735 : #if HAL_TORQEEDO_ENABLED
736 : case AUX_FUNC::TORQEEDO_CLEAR_ERR:
737 : #endif
738 : #if AP_SCRIPTING_ENABLED
739 : case AUX_FUNC::SCRIPTING_1:
740 : case AUX_FUNC::SCRIPTING_2:
741 : case AUX_FUNC::SCRIPTING_3:
742 : case AUX_FUNC::SCRIPTING_4:
743 : case AUX_FUNC::SCRIPTING_5:
744 : case AUX_FUNC::SCRIPTING_6:
745 : case AUX_FUNC::SCRIPTING_7:
746 : case AUX_FUNC::SCRIPTING_8:
747 : case AUX_FUNC::SCRIPTING_9:
748 : case AUX_FUNC::SCRIPTING_10:
749 : case AUX_FUNC::SCRIPTING_11:
750 : case AUX_FUNC::SCRIPTING_12:
751 : case AUX_FUNC::SCRIPTING_13:
752 : case AUX_FUNC::SCRIPTING_14:
753 : case AUX_FUNC::SCRIPTING_15:
754 : case AUX_FUNC::SCRIPTING_16:
755 : case AUX_FUNC::STOP_RESTART_SCRIPTING:
756 : #endif
757 : #if AP_VIDEOTX_ENABLED
758 : case AUX_FUNC::VTX_POWER:
759 : #endif
760 : #if AP_OPTICALFLOW_CALIBRATOR_ENABLED
761 : case AUX_FUNC::OPTFLOW_CAL:
762 : #endif
763 : case AUX_FUNC::TURBINE_START:
764 : #if HAL_MOUNT_ENABLED
765 : case AUX_FUNC::MOUNT1_ROLL:
766 : case AUX_FUNC::MOUNT1_PITCH:
767 : case AUX_FUNC::MOUNT1_YAW:
768 : case AUX_FUNC::MOUNT2_ROLL:
769 : case AUX_FUNC::MOUNT2_PITCH:
770 : case AUX_FUNC::MOUNT2_YAW:
771 : #endif
772 : #if HAL_GENERATOR_ENABLED
773 : case AUX_FUNC::LOWEHEISER_STARTER:
774 : #endif
775 : #if COMPASS_CAL_ENABLED
776 : case AUX_FUNC::MAG_CAL:
777 : #endif
778 : #if AP_CAMERA_ENABLED
779 : case AUX_FUNC::CAMERA_IMAGE_TRACKING:
780 : #endif
781 : #if HAL_MOUNT_ENABLED
782 : case AUX_FUNC::MOUNT_LRF_ENABLE:
783 : #endif
784 : #if HAL_GENERATOR_ENABLED
785 : case AUX_FUNC::LOWEHEISER_THROTTLE:
786 : #endif
787 32406 : break;
788 :
789 : // these functions require explicit initialization
790 : #if HAL_ADSB_ENABLED
791 19 : case AUX_FUNC::AVOID_ADSB:
792 : #endif
793 : case AUX_FUNC::AVOID_PROXIMITY:
794 : #if AP_FENCE_ENABLED
795 : case AUX_FUNC::FENCE:
796 : #endif
797 : #if AP_GPS_ENABLED
798 : case AUX_FUNC::GPS_DISABLE:
799 : case AUX_FUNC::GPS_DISABLE_YAW:
800 : #endif
801 : #if AP_INERTIALSENSOR_KILL_IMU_ENABLED
802 : case AUX_FUNC::KILL_IMU1:
803 : case AUX_FUNC::KILL_IMU2:
804 : case AUX_FUNC::KILL_IMU3:
805 : #endif
806 : #if AP_MISSION_ENABLED
807 : case AUX_FUNC::MISSION_RESET:
808 : #endif
809 : case AUX_FUNC::MOTOR_ESTOP:
810 : case AUX_FUNC::RC_OVERRIDE_ENABLE:
811 : #if AP_CAMERA_RUNCAM_ENABLED
812 : case AUX_FUNC::RUNCAM_CONTROL:
813 : case AUX_FUNC::RUNCAM_OSD_CONTROL:
814 : #endif
815 : #if HAL_SPRAYER_ENABLED
816 : case AUX_FUNC::SPRAYER:
817 : #endif
818 : #if AP_AIRSPEED_ENABLED
819 : case AUX_FUNC::DISABLE_AIRSPEED_USE:
820 : #endif
821 : case AUX_FUNC::FFT_NOTCH_TUNE:
822 : #if HAL_MOUNT_ENABLED
823 : case AUX_FUNC::RETRACT_MOUNT1:
824 : case AUX_FUNC::RETRACT_MOUNT2:
825 : case AUX_FUNC::MOUNT_YAW_LOCK:
826 : case AUX_FUNC::MOUNT_RP_LOCK:
827 : #if AP_MOUNT_POI_LOCK_ENABLED
828 : case AUX_FUNC::MOUNT_POI_LOCK:
829 : #endif //AP_MOUNT_POI_LOCK_ENABLED
830 : #endif //HAL_MOUNT_ENABLED
831 : #if HAL_LOGGING_ENABLED
832 : case AUX_FUNC::LOG_PAUSE:
833 : #endif
834 : case AUX_FUNC::ARM_EMERGENCY_STOP:
835 : #if AP_CAMERA_ENABLED
836 : case AUX_FUNC::CAMERA_REC_VIDEO:
837 : case AUX_FUNC::CAMERA_ZOOM:
838 : case AUX_FUNC::CAMERA_MANUAL_FOCUS:
839 : case AUX_FUNC::CAMERA_AUTO_FOCUS:
840 : case AUX_FUNC::CAMERA_LENS:
841 : #endif
842 : #if AP_AHRS_ENABLED
843 : case AUX_FUNC::AHRS_TYPE:
844 : #endif
845 19 : run_aux_function(ch_option, ch_flag, AuxFuncTrigger::Source::INIT, ch_in);
846 19 : break;
847 0 : default:
848 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to init: RC%u_OPTION: %u",
849 : (unsigned)(this->ch_in+1), (unsigned)ch_option);
850 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
851 0 : AP_BoardConfig::config_error("Failed to init: RC%u_OPTION: %u",
852 0 : (unsigned)(this->ch_in+1), (unsigned)ch_option);
853 : #endif
854 0 : break;
855 : }
856 32425 : }
857 :
858 : #if AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED
859 :
860 : const RC_Channel::LookupTable RC_Channel::lookuptable[] = {
861 : #if AP_MISSION_ENABLED
862 : { AUX_FUNC::SAVE_WP,"SaveWaypoint"},
863 : #endif
864 : #if AP_CAMERA_ENABLED
865 : { AUX_FUNC::CAMERA_TRIGGER,"CameraTrigger"},
866 : #endif
867 : #if AP_RANGEFINDER_ENABLED
868 : { AUX_FUNC::RANGEFINDER,"Rangefinder"},
869 : #endif
870 : #if AP_FENCE_ENABLED
871 : { AUX_FUNC::FENCE,"Fence"},
872 : #endif
873 : #if HAL_SPRAYER_ENABLED
874 : { AUX_FUNC::SPRAYER,"Sprayer"},
875 : #endif
876 : #if HAL_PARACHUTE_ENABLED
877 : { AUX_FUNC::PARACHUTE_ENABLE,"ParachuteEnable"},
878 : { AUX_FUNC::PARACHUTE_RELEASE,"ParachuteRelease"},
879 : { AUX_FUNC::PARACHUTE_3POS,"Parachute3Position"},
880 : #endif
881 : #if AP_MISSION_ENABLED
882 : { AUX_FUNC::MISSION_RESET,"MissionReset"},
883 : #endif
884 : #if HAL_MOUNT_ENABLED
885 : { AUX_FUNC::RETRACT_MOUNT1,"RetractMount1"},
886 : { AUX_FUNC::RETRACT_MOUNT2,"RetractMount2"},
887 : #endif
888 : #if AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED
889 : { AUX_FUNC::RELAY,"Relay1"},
890 : #endif
891 : { AUX_FUNC::MOTOR_ESTOP,"MotorEStop"},
892 : { AUX_FUNC::MOTOR_INTERLOCK,"MotorInterlock"},
893 : #if AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED
894 : { AUX_FUNC::RELAY2,"Relay2"},
895 : { AUX_FUNC::RELAY3,"Relay3"},
896 : { AUX_FUNC::RELAY4,"Relay4"},
897 : #endif
898 : { AUX_FUNC::PRECISION_LOITER,"PrecisionLoiter"},
899 : { AUX_FUNC::AVOID_PROXIMITY,"AvoidProximity"},
900 : #if AP_WINCH_ENABLED
901 : { AUX_FUNC::WINCH_ENABLE,"WinchEnable"},
902 : { AUX_FUNC::WINCH_CONTROL,"WinchControl"},
903 : #endif
904 : #if AP_MISSION_ENABLED
905 : { AUX_FUNC::CLEAR_WP,"ClearWaypoint"},
906 : #endif
907 : { AUX_FUNC::COMPASS_LEARN,"CompassLearn"},
908 : { AUX_FUNC::SAILBOAT_TACK,"SailboatTack"},
909 : #if AP_GPS_ENABLED
910 : { AUX_FUNC::GPS_DISABLE,"GPSDisable"},
911 : { AUX_FUNC::GPS_DISABLE_YAW,"GPSDisableYaw"},
912 : #endif
913 : #if AP_AIRSPEED_ENABLED
914 : { AUX_FUNC::DISABLE_AIRSPEED_USE,"DisableAirspeedUse"},
915 : #endif
916 : #if AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED
917 : { AUX_FUNC::RELAY5,"Relay5"},
918 : { AUX_FUNC::RELAY6,"Relay6"},
919 : #endif
920 : { AUX_FUNC::SAILBOAT_MOTOR_3POS,"SailboatMotor"},
921 : { AUX_FUNC::SURFACE_TRACKING,"SurfaceTracking"},
922 : #if AP_CAMERA_RUNCAM_ENABLED
923 : { AUX_FUNC::RUNCAM_CONTROL,"RunCamControl"},
924 : { AUX_FUNC::RUNCAM_OSD_CONTROL,"RunCamOSDControl"},
925 : #endif
926 : #if HAL_VISUALODOM_ENABLED
927 : { AUX_FUNC::VISODOM_ALIGN,"VisOdomAlign"},
928 : #endif
929 : { AUX_FUNC::AIRMODE, "AirMode"},
930 : #if AP_CAMERA_ENABLED
931 : { AUX_FUNC::CAM_MODE_TOGGLE,"CamModeToggle"},
932 : #endif
933 : #if HAL_GENERATOR_ENABLED
934 : { AUX_FUNC::GENERATOR,"Generator"},
935 : #endif
936 : #if AP_BATTERY_ENABLED
937 : { AUX_FUNC::BATTERY_MPPT_ENABLE,"Battery MPPT Enable"},
938 : #endif
939 : #if AP_AIRSPEED_AUTOCAL_ENABLE
940 : { AUX_FUNC::ARSPD_CALIBRATE,"Calibrate Airspeed"},
941 : #endif
942 : #if HAL_TORQEEDO_ENABLED
943 : { AUX_FUNC::TORQEEDO_CLEAR_ERR, "Torqeedo Clear Err"},
944 : #endif
945 : { AUX_FUNC::EMERGENCY_LANDING_EN, "Emergency Landing"},
946 : { AUX_FUNC::WEATHER_VANE_ENABLE, "Weathervane"},
947 : { AUX_FUNC::TURBINE_START, "Turbine Start"},
948 : { AUX_FUNC::FFT_NOTCH_TUNE, "FFT Notch Tuning"},
949 : #if HAL_MOUNT_ENABLED
950 : { AUX_FUNC::MOUNT_YAW_LOCK, "Mount Yaw Lock"},
951 : { AUX_FUNC::MOUNT_RP_LOCK, "Mount Roll/Pitch Lock"},
952 : #endif //HAL_MOUNT_ENABLED
953 : #if HAL_LOGGING_ENABLED
954 : { AUX_FUNC::LOG_PAUSE, "Pause Stream Logging"},
955 : #endif
956 : #if AP_CAMERA_ENABLED
957 : { AUX_FUNC::CAMERA_REC_VIDEO, "Camera Record Video"},
958 : { AUX_FUNC::CAMERA_ZOOM, "Camera Zoom"},
959 : { AUX_FUNC::CAMERA_MANUAL_FOCUS, "Camera Manual Focus"},
960 : { AUX_FUNC::CAMERA_AUTO_FOCUS, "Camera Auto Focus"},
961 : { AUX_FUNC::CAMERA_IMAGE_TRACKING, "Camera Image Tracking"},
962 : { AUX_FUNC::CAMERA_LENS, "Camera Lens"},
963 : #endif
964 : #if HAL_MOUNT_ENABLED
965 : { AUX_FUNC::MOUNT_LRF_ENABLE, "Mount LRF Enable"},
966 : #endif
967 : };
968 :
969 : /* lookup the announcement for switch change */
970 1968 : const char *RC_Channel::string_for_aux_function(AUX_FUNC function) const
971 : {
972 44524 : for (const struct LookupTable &entry : lookuptable) {
973 43856 : if (entry.option == function) {
974 1300 : return entry.announcement;
975 : }
976 : }
977 668 : return nullptr;
978 : }
979 :
980 : /* find string for position */
981 1300 : const char *RC_Channel::string_for_aux_pos(AuxSwitchPos pos) const
982 : {
983 1300 : switch (pos) {
984 132 : case AuxSwitchPos::HIGH:
985 132 : return "HIGH";
986 940 : case AuxSwitchPos::MIDDLE:
987 940 : return "MIDDLE";
988 228 : case AuxSwitchPos::LOW:
989 228 : return "LOW";
990 : }
991 0 : return "";
992 : }
993 :
994 : #endif // AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED
995 :
996 : /*
997 : read an aux channel. Return true if a switch has changed
998 : */
999 23606768 : bool RC_Channel::read_aux()
1000 : {
1001 23606768 : const AUX_FUNC _option = (AUX_FUNC)option.get();
1002 23606768 : if (_option == AUX_FUNC::DO_NOTHING) {
1003 : // may wish to add special cases for other "AUXSW" things
1004 : // here e.g. RCMAP_ROLL etc once they become options
1005 22599013 : return false;
1006 : #if AP_VIDEOTX_ENABLED
1007 1007755 : } else if (_option == AUX_FUNC::VTX_POWER) {
1008 : int8_t position;
1009 0 : if (read_6pos_switch(position)) {
1010 0 : AP::vtx().change_power(position);
1011 0 : return true;
1012 : }
1013 0 : return false;
1014 : #endif // AP_VIDEOTX_ENABLED
1015 : }
1016 :
1017 : AuxSwitchPos new_position;
1018 1007755 : if (!read_3pos_switch(new_position)) {
1019 714 : return false;
1020 : }
1021 :
1022 1007041 : if (!switch_state.initialised) {
1023 1403 : switch_state.initialised = true;
1024 1403 : if (init_position_on_first_radio_read((AUX_FUNC)option.get())) {
1025 14 : switch_state.current_position = (int8_t)new_position;
1026 14 : switch_state.debounce_position = (int8_t)new_position;
1027 : }
1028 : }
1029 :
1030 1007041 : if (!debounce_completed((int8_t)new_position)) {
1031 1005085 : return false;
1032 : }
1033 :
1034 : #if AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED
1035 : // announce the change to the GCS:
1036 1956 : const char *aux_string = string_for_aux_function(_option);
1037 1956 : if (aux_string != nullptr) {
1038 1296 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "RC%i: %s %s", ch_in+1, aux_string, string_for_aux_pos(new_position));
1039 : }
1040 : #endif
1041 :
1042 : // debounced; undertake the action:
1043 1956 : run_aux_function(_option, new_position, AuxFuncTrigger::Source::RC, ch_in);
1044 1956 : return true;
1045 : }
1046 :
1047 : // returns true if the first time we successfully read the
1048 : // channel's three-position-switch position we should record that
1049 : // position as the current position *without* executing the
1050 : // associated auxiliary function. e.g. do not attempt to arm a
1051 : // vehicle when the user turns on their transmitter with the arm
1052 : // switch high!
1053 1403 : bool RC_Channel::init_position_on_first_radio_read(AUX_FUNC func) const
1054 : {
1055 1403 : switch (func) {
1056 : #if AP_ARMING_ENABLED
1057 14 : case AUX_FUNC::ARMDISARM_AIRMODE:
1058 : case AUX_FUNC::ARMDISARM:
1059 : case AUX_FUNC::ARM_EMERGENCY_STOP:
1060 : #endif
1061 : #if HAL_PARACHUTE_ENABLED
1062 : case AUX_FUNC::PARACHUTE_RELEASE:
1063 : #endif
1064 :
1065 : // we do not want to process
1066 14 : return true;
1067 1389 : default:
1068 1389 : return false;
1069 : }
1070 : }
1071 :
1072 69 : void RC_Channel::do_aux_function_armdisarm(const AuxSwitchPos ch_flag)
1073 : {
1074 : // arm or disarm the vehicle
1075 69 : switch (ch_flag) {
1076 32 : case AuxSwitchPos::HIGH:
1077 32 : AP::arming().arm(AP_Arming::Method::AUXSWITCH, true);
1078 32 : break;
1079 0 : case AuxSwitchPos::MIDDLE:
1080 : // nothing
1081 0 : break;
1082 37 : case AuxSwitchPos::LOW:
1083 37 : AP::arming().disarm(AP_Arming::Method::AUXSWITCH);
1084 37 : break;
1085 : }
1086 69 : }
1087 :
1088 : #if AP_ADSB_AVOIDANCE_ENABLED
1089 5 : void RC_Channel::do_aux_function_avoid_adsb(const AuxSwitchPos ch_flag)
1090 : {
1091 5 : AP_Avoidance *avoidance = AP::ap_avoidance();
1092 5 : if (avoidance == nullptr) {
1093 0 : return;
1094 : }
1095 5 : if (ch_flag == AuxSwitchPos::HIGH) {
1096 3 : AP_ADSB *adsb = AP::ADSB();
1097 3 : if (adsb == nullptr) {
1098 0 : return;
1099 : }
1100 : // try to enable AP_Avoidance
1101 3 : if (!adsb->enabled() || !adsb->healthy()) {
1102 1 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ADSB not available");
1103 1 : return;
1104 : }
1105 2 : avoidance->enable();
1106 2 : LOGGER_WRITE_EVENT(LogEvent::AVOIDANCE_ADSB_ENABLE);
1107 2 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ADSB Avoidance Enabled");
1108 2 : return;
1109 : }
1110 :
1111 : // disable AP_Avoidance
1112 2 : avoidance->disable();
1113 2 : LOGGER_WRITE_EVENT(LogEvent::AVOIDANCE_ADSB_DISABLE);
1114 2 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ADSB Avoidance Disabled");
1115 : }
1116 : #endif // AP_ADSB_AVOIDANCE_ENABLED
1117 :
1118 11 : void RC_Channel::do_aux_function_avoid_proximity(const AuxSwitchPos ch_flag)
1119 : {
1120 : #if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1121 11 : AC_Avoid *avoid = AP::ac_avoid();
1122 11 : if (avoid == nullptr) {
1123 0 : return;
1124 : }
1125 :
1126 11 : switch (ch_flag) {
1127 3 : case AuxSwitchPos::HIGH:
1128 3 : avoid->proximity_avoidance_enable(true);
1129 3 : break;
1130 5 : case AuxSwitchPos::MIDDLE:
1131 : // nothing
1132 5 : break;
1133 3 : case AuxSwitchPos::LOW:
1134 3 : avoid->proximity_avoidance_enable(false);
1135 3 : break;
1136 : }
1137 : #endif // !APM_BUILD_ArduPlane
1138 0 : }
1139 :
1140 : #if AP_CAMERA_ENABLED
1141 16 : void RC_Channel::do_aux_function_camera_trigger(const AuxSwitchPos ch_flag)
1142 : {
1143 16 : if (ch_flag == AuxSwitchPos::HIGH) {
1144 6 : AP_Camera *camera = AP::camera();
1145 6 : if (camera == nullptr) {
1146 0 : return;
1147 : }
1148 6 : camera->take_picture();
1149 : }
1150 : }
1151 :
1152 0 : bool RC_Channel::do_aux_function_record_video(const AuxSwitchPos ch_flag)
1153 : {
1154 0 : AP_Camera *camera = AP::camera();
1155 0 : if (camera == nullptr) {
1156 0 : return false;
1157 : }
1158 0 : return camera->record_video(ch_flag == AuxSwitchPos::HIGH);
1159 : }
1160 :
1161 0 : bool RC_Channel::do_aux_function_camera_zoom(const AuxSwitchPos ch_flag)
1162 : {
1163 0 : AP_Camera *camera = AP::camera();
1164 0 : if (camera == nullptr) {
1165 0 : return false;
1166 : }
1167 0 : int8_t zoom_step = 0; // zoom out = -1, hold = 0, zoom in = 1
1168 0 : switch (ch_flag) {
1169 0 : case AuxSwitchPos::HIGH:
1170 0 : zoom_step = 1; // zoom in
1171 0 : break;
1172 0 : case AuxSwitchPos::MIDDLE:
1173 0 : zoom_step = 0; // zoom hold
1174 0 : break;
1175 0 : case AuxSwitchPos::LOW:
1176 0 : zoom_step = -1; // zoom out
1177 0 : break;
1178 : }
1179 0 : return camera->set_zoom(ZoomType::RATE, zoom_step);
1180 : }
1181 :
1182 0 : bool RC_Channel::do_aux_function_camera_manual_focus(const AuxSwitchPos ch_flag)
1183 : {
1184 0 : AP_Camera *camera = AP::camera();
1185 0 : if (camera == nullptr) {
1186 0 : return false;
1187 : }
1188 0 : int8_t focus_step = 0; // focus in = -1, focus hold = 0, focus out = 1
1189 0 : switch (ch_flag) {
1190 0 : case AuxSwitchPos::HIGH:
1191 : // wide shot, focus out
1192 0 : focus_step = 1;
1193 0 : break;
1194 0 : case AuxSwitchPos::MIDDLE:
1195 0 : focus_step = 0;
1196 0 : break;
1197 0 : case AuxSwitchPos::LOW:
1198 : // close shot, focus in
1199 0 : focus_step = -1;
1200 0 : break;
1201 : }
1202 0 : return camera->set_focus(FocusType::RATE, focus_step) == SetFocusResult::ACCEPTED;
1203 : }
1204 :
1205 0 : bool RC_Channel::do_aux_function_camera_auto_focus(const AuxSwitchPos ch_flag)
1206 : {
1207 0 : if (ch_flag == AuxSwitchPos::HIGH) {
1208 0 : AP_Camera *camera = AP::camera();
1209 0 : if (camera == nullptr) {
1210 0 : return false;
1211 : }
1212 0 : return camera->set_focus(FocusType::AUTO, 0) == SetFocusResult::ACCEPTED;
1213 : }
1214 0 : return false;
1215 : }
1216 :
1217 0 : bool RC_Channel::do_aux_function_camera_image_tracking(const AuxSwitchPos ch_flag)
1218 : {
1219 0 : AP_Camera *camera = AP::camera();
1220 0 : if (camera == nullptr) {
1221 0 : return false;
1222 : }
1223 : // High position enables tracking a POINT in middle of image
1224 : // Low or Mediums disables tracking. (0.5,0.5) is still passed in but ignored
1225 0 : return camera->set_tracking(ch_flag == AuxSwitchPos::HIGH ? TrackingType::TRK_POINT : TrackingType::TRK_NONE, Vector2f{0.5, 0.5}, Vector2f{});
1226 : }
1227 :
1228 0 : bool RC_Channel::do_aux_function_camera_lens(const AuxSwitchPos ch_flag)
1229 : {
1230 : #if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
1231 0 : AP_Camera *camera = AP::camera();
1232 0 : if (camera == nullptr) {
1233 0 : return false;
1234 : }
1235 : // Low selects lens 0 (default), Mediums selects lens1, High selects lens2
1236 0 : return camera->set_lens((uint8_t)ch_flag);
1237 : #else
1238 : return false;
1239 : #endif // AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
1240 : }
1241 : #endif // AP_CAMERA_ENABLED
1242 :
1243 : #if AP_CAMERA_RUNCAM_ENABLED
1244 0 : void RC_Channel::do_aux_function_runcam_control(const AuxSwitchPos ch_flag)
1245 : {
1246 0 : AP_RunCam *runcam = AP::runcam();
1247 0 : if (runcam == nullptr) {
1248 0 : return;
1249 : }
1250 :
1251 0 : switch (ch_flag) {
1252 0 : case AuxSwitchPos::HIGH:
1253 0 : runcam->start_recording();
1254 0 : break;
1255 0 : case AuxSwitchPos::MIDDLE:
1256 0 : runcam->osd_option();
1257 0 : break;
1258 0 : case AuxSwitchPos::LOW:
1259 0 : runcam->stop_recording();
1260 0 : break;
1261 : }
1262 : }
1263 :
1264 0 : void RC_Channel::do_aux_function_runcam_osd_control(const AuxSwitchPos ch_flag)
1265 : {
1266 0 : AP_RunCam *runcam = AP::runcam();
1267 0 : if (runcam == nullptr) {
1268 0 : return;
1269 : }
1270 :
1271 0 : switch (ch_flag) {
1272 0 : case AuxSwitchPos::HIGH:
1273 0 : runcam->enter_osd();
1274 0 : break;
1275 0 : case AuxSwitchPos::MIDDLE:
1276 : case AuxSwitchPos::LOW:
1277 0 : runcam->exit_osd();
1278 0 : break;
1279 : }
1280 : }
1281 : #endif
1282 :
1283 : #if AP_FENCE_ENABLED
1284 : // enable or disable the fence
1285 14 : void RC_Channel::do_aux_function_fence(const AuxSwitchPos ch_flag)
1286 : {
1287 14 : AC_Fence *fence = AP::fence();
1288 14 : if (fence == nullptr) {
1289 0 : return;
1290 : }
1291 :
1292 14 : fence->enable_configured(ch_flag == AuxSwitchPos::HIGH);
1293 : }
1294 : #endif
1295 :
1296 : #if AP_MISSION_ENABLED
1297 9 : void RC_Channel::do_aux_function_clear_wp(const AuxSwitchPos ch_flag)
1298 : {
1299 9 : if (ch_flag == AuxSwitchPos::HIGH) {
1300 3 : AP::mission().clear();
1301 : }
1302 9 : }
1303 : #endif // AP_MISSION_ENABLED
1304 :
1305 : #if AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED
1306 9 : void RC_Channel::do_aux_function_relay(const uint8_t relay, bool val)
1307 : {
1308 9 : AP_ServoRelayEvents *servorelayevents = AP::servorelayevents();
1309 9 : if (servorelayevents == nullptr) {
1310 0 : return;
1311 : }
1312 9 : servorelayevents->do_set_relay(relay, val);
1313 : }
1314 : #endif
1315 :
1316 : #if HAL_GENERATOR_ENABLED
1317 46 : void RC_Channel::do_aux_function_generator(const AuxSwitchPos ch_flag)
1318 : {
1319 46 : AP_Generator *generator = AP::generator();
1320 46 : if (generator == nullptr) {
1321 0 : return;
1322 : }
1323 :
1324 46 : switch (ch_flag) {
1325 17 : case AuxSwitchPos::LOW:
1326 17 : generator->stop();
1327 17 : break;
1328 13 : case AuxSwitchPos::MIDDLE:
1329 13 : generator->idle();
1330 13 : break;
1331 16 : case AuxSwitchPos::HIGH:
1332 16 : generator->run();
1333 16 : break;
1334 : }
1335 : }
1336 : #endif
1337 :
1338 : #if HAL_SPRAYER_ENABLED
1339 14 : void RC_Channel::do_aux_function_sprayer(const AuxSwitchPos ch_flag)
1340 : {
1341 14 : AC_Sprayer *sprayer = AP::sprayer();
1342 14 : if (sprayer == nullptr) {
1343 0 : return;
1344 : }
1345 14 : sprayer->run(ch_flag == AuxSwitchPos::HIGH);
1346 : // if we are disarmed the pilot must want to test the pump
1347 14 : sprayer->test_pump((ch_flag == AuxSwitchPos::HIGH) && !hal.util->get_soft_armed());
1348 : }
1349 : #endif // HAL_SPRAYER_ENABLED
1350 :
1351 : #if AP_GRIPPER_ENABLED
1352 30 : void RC_Channel::do_aux_function_gripper(const AuxSwitchPos ch_flag)
1353 : {
1354 30 : AP_Gripper &gripper = AP::gripper();
1355 :
1356 30 : switch (ch_flag) {
1357 9 : case AuxSwitchPos::LOW:
1358 9 : gripper.release();
1359 9 : break;
1360 11 : case AuxSwitchPos::MIDDLE:
1361 : // nothing
1362 11 : break;
1363 10 : case AuxSwitchPos::HIGH:
1364 10 : gripper.grab();
1365 10 : break;
1366 : }
1367 30 : }
1368 : #endif // AP_GRIPPER_ENABLED
1369 :
1370 0 : void RC_Channel::do_aux_function_lost_vehicle_sound(const AuxSwitchPos ch_flag)
1371 : {
1372 0 : switch (ch_flag) {
1373 0 : case AuxSwitchPos::HIGH:
1374 0 : AP_Notify::flags.vehicle_lost = true;
1375 0 : break;
1376 0 : case AuxSwitchPos::MIDDLE:
1377 : // nothing
1378 0 : break;
1379 0 : case AuxSwitchPos::LOW:
1380 0 : AP_Notify::flags.vehicle_lost = false;
1381 0 : break;
1382 : }
1383 0 : }
1384 :
1385 47 : void RC_Channel::do_aux_function_rc_override_enable(const AuxSwitchPos ch_flag)
1386 : {
1387 47 : switch (ch_flag) {
1388 19 : case AuxSwitchPos::HIGH: {
1389 19 : rc().set_gcs_overrides_enabled(true);
1390 19 : break;
1391 : }
1392 9 : case AuxSwitchPos::MIDDLE:
1393 : // nothing
1394 9 : break;
1395 19 : case AuxSwitchPos::LOW: {
1396 19 : rc().set_gcs_overrides_enabled(false);
1397 19 : break;
1398 : }
1399 : }
1400 47 : }
1401 :
1402 : #if AP_MISSION_ENABLED
1403 2 : void RC_Channel::do_aux_function_mission_reset(const AuxSwitchPos ch_flag)
1404 : {
1405 2 : if (ch_flag != AuxSwitchPos::HIGH) {
1406 1 : return;
1407 : }
1408 1 : AP::mission().reset();
1409 : }
1410 : #endif
1411 :
1412 8 : void RC_Channel::do_aux_function_fft_notch_tune(const AuxSwitchPos ch_flag)
1413 : {
1414 : #if HAL_GYROFFT_ENABLED
1415 8 : AP_GyroFFT *fft = AP::fft();
1416 8 : if (fft == nullptr) {
1417 0 : return;
1418 : }
1419 :
1420 8 : switch (ch_flag) {
1421 2 : case AuxSwitchPos::HIGH:
1422 2 : fft->start_notch_tune();
1423 2 : break;
1424 6 : case AuxSwitchPos::MIDDLE:
1425 : case AuxSwitchPos::LOW:
1426 6 : fft->stop_notch_tune();
1427 6 : break;
1428 : }
1429 : #endif
1430 0 : }
1431 :
1432 : /**
1433 : * Perform the RETRACT_MOUNT 1/2 process.
1434 : *
1435 : * @param [in] ch_flag Position of the switch. HIGH, MIDDLE and LOW.
1436 : * @param [in] instance 0: RETRACT MOUNT 1 <br>
1437 : * 1: RETRACT MOUNT 2
1438 : */
1439 : #if HAL_MOUNT_ENABLED
1440 0 : void RC_Channel::do_aux_function_retract_mount(const AuxSwitchPos ch_flag, const uint8_t instance)
1441 : {
1442 0 : AP_Mount *mount = AP::mount();
1443 0 : if (mount == nullptr) {
1444 0 : return;
1445 : }
1446 0 : switch (ch_flag) {
1447 0 : case AuxSwitchPos::HIGH:
1448 0 : mount->set_mode(instance,MAV_MOUNT_MODE_RETRACT);
1449 0 : break;
1450 0 : case AuxSwitchPos::MIDDLE:
1451 : // nothing
1452 0 : break;
1453 0 : case AuxSwitchPos::LOW:
1454 0 : mount->set_mode_to_default(instance);
1455 0 : break;
1456 : }
1457 : }
1458 : #endif // HAL_MOUNT_ENABLED
1459 :
1460 2139 : bool RC_Channel::run_aux_function(AUX_FUNC ch_option, AuxSwitchPos pos, AuxFuncTrigger::Source source, uint16_t source_index)
1461 : {
1462 : #if AP_SCRIPTING_ENABLED
1463 2139 : rc().set_aux_cached(ch_option, pos);
1464 : #endif
1465 :
1466 : const AuxFuncTrigger trigger {
1467 : func: ch_option,
1468 : pos: pos,
1469 : source: source,
1470 : source_index: source_index,
1471 2139 : };
1472 :
1473 2139 : const bool ret = do_aux_function(trigger);
1474 :
1475 : #if HAL_LOGGING_ENABLED
1476 : // @LoggerMessage: AUXF
1477 : // @Description: Auxiliary function invocation information
1478 : // @Field: TimeUS: Time since system startup
1479 : // @Field: function: ID of triggered function
1480 : // @FieldValueEnum: function: RC_Channel::AUX_FUNC
1481 : // @Field: pos: switch position when function triggered
1482 : // @FieldValueEnum: pos: RC_Channel::AuxSwitchPos
1483 : // @Field: source: source of auxiliary function invocation
1484 : // @FieldValueEnum: source: RC_Channel::AuxFuncTrigger::Source
1485 : // @Field: index: index within source. 0 indexed. Invalid for scripting.
1486 : // @Field: result: true if function was successful
1487 2139 : AP::logger().Write(
1488 : "AUXF",
1489 : "TimeUS,function,pos,source,index,result",
1490 : "s#----",
1491 : "F-----",
1492 : "QHBBHB",
1493 : AP_HAL::micros64(),
1494 : uint16_t(ch_option),
1495 : uint8_t(pos),
1496 : uint8_t(source),
1497 : source_index,
1498 : uint8_t(ret)
1499 : );
1500 : #endif
1501 :
1502 2139 : return ret;
1503 : }
1504 :
1505 618 : bool RC_Channel::do_aux_function(const AuxFuncTrigger &trigger)
1506 : {
1507 618 : const AUX_FUNC &ch_option = trigger.func;
1508 618 : const AuxSwitchPos &ch_flag = trigger.pos;
1509 :
1510 618 : switch (ch_option) {
1511 : #if AP_FENCE_ENABLED
1512 14 : case AUX_FUNC::FENCE:
1513 14 : do_aux_function_fence(ch_flag);
1514 14 : break;
1515 : #endif
1516 :
1517 : #if AP_GRIPPER_ENABLED
1518 30 : case AUX_FUNC::GRIPPER:
1519 30 : do_aux_function_gripper(ch_flag);
1520 30 : break;
1521 : #endif
1522 :
1523 47 : case AUX_FUNC::RC_OVERRIDE_ENABLE:
1524 : // Allow or disallow RC_Override
1525 47 : do_aux_function_rc_override_enable(ch_flag);
1526 47 : break;
1527 :
1528 11 : case AUX_FUNC::AVOID_PROXIMITY:
1529 11 : do_aux_function_avoid_proximity(ch_flag);
1530 11 : break;
1531 :
1532 : #if AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED
1533 9 : case AUX_FUNC::RELAY:
1534 9 : do_aux_function_relay(0, ch_flag == AuxSwitchPos::HIGH);
1535 9 : break;
1536 0 : case AUX_FUNC::RELAY2:
1537 0 : do_aux_function_relay(1, ch_flag == AuxSwitchPos::HIGH);
1538 0 : break;
1539 0 : case AUX_FUNC::RELAY3:
1540 0 : do_aux_function_relay(2, ch_flag == AuxSwitchPos::HIGH);
1541 0 : break;
1542 0 : case AUX_FUNC::RELAY4:
1543 0 : do_aux_function_relay(3, ch_flag == AuxSwitchPos::HIGH);
1544 0 : break;
1545 0 : case AUX_FUNC::RELAY5:
1546 0 : do_aux_function_relay(4, ch_flag == AuxSwitchPos::HIGH);
1547 0 : break;
1548 0 : case AUX_FUNC::RELAY6:
1549 0 : do_aux_function_relay(5, ch_flag == AuxSwitchPos::HIGH);
1550 0 : break;
1551 : #endif // AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED
1552 :
1553 : #if AP_CAMERA_RUNCAM_ENABLED
1554 0 : case AUX_FUNC::RUNCAM_CONTROL:
1555 0 : do_aux_function_runcam_control(ch_flag);
1556 0 : break;
1557 :
1558 0 : case AUX_FUNC::RUNCAM_OSD_CONTROL:
1559 0 : do_aux_function_runcam_osd_control(ch_flag);
1560 0 : break;
1561 : #endif
1562 :
1563 : #if AP_MISSION_ENABLED
1564 9 : case AUX_FUNC::CLEAR_WP:
1565 9 : do_aux_function_clear_wp(ch_flag);
1566 9 : break;
1567 2 : case AUX_FUNC::MISSION_RESET:
1568 2 : do_aux_function_mission_reset(ch_flag);
1569 2 : break;
1570 : #endif
1571 :
1572 : #if AP_ADSB_AVOIDANCE_ENABLED
1573 5 : case AUX_FUNC::AVOID_ADSB:
1574 5 : do_aux_function_avoid_adsb(ch_flag);
1575 5 : break;
1576 : #endif // AP_ADSB_AVOIDANCE_ENABLED
1577 :
1578 8 : case AUX_FUNC::FFT_NOTCH_TUNE:
1579 8 : do_aux_function_fft_notch_tune(ch_flag);
1580 8 : break;
1581 :
1582 : #if HAL_GENERATOR_ENABLED
1583 46 : case AUX_FUNC::GENERATOR:
1584 46 : do_aux_function_generator(ch_flag);
1585 46 : break;
1586 : #endif
1587 :
1588 : #if AP_BATTERY_ENABLED
1589 0 : case AUX_FUNC::BATTERY_MPPT_ENABLE:
1590 0 : if (ch_flag != AuxSwitchPos::MIDDLE) {
1591 0 : AP::battery().set_powered_state_to_all(ch_flag == AuxSwitchPos::HIGH);
1592 : }
1593 0 : break;
1594 : #endif
1595 :
1596 : #if HAL_SPRAYER_ENABLED
1597 14 : case AUX_FUNC::SPRAYER:
1598 14 : do_aux_function_sprayer(ch_flag);
1599 14 : break;
1600 : #endif
1601 :
1602 0 : case AUX_FUNC::LOST_VEHICLE_SOUND:
1603 0 : do_aux_function_lost_vehicle_sound(ch_flag);
1604 0 : break;
1605 :
1606 : #if AP_ARMING_ENABLED
1607 33 : case AUX_FUNC::ARMDISARM:
1608 33 : do_aux_function_armdisarm(ch_flag);
1609 33 : break;
1610 :
1611 0 : case AUX_FUNC::DISARM:
1612 0 : if (ch_flag == AuxSwitchPos::HIGH) {
1613 0 : AP::arming().disarm(AP_Arming::Method::AUXSWITCH);
1614 : }
1615 0 : break;
1616 : #endif
1617 :
1618 0 : case AUX_FUNC::COMPASS_LEARN:
1619 0 : if (ch_flag == AuxSwitchPos::HIGH) {
1620 0 : Compass &compass = AP::compass();
1621 0 : compass.set_learn_type(Compass::LearnType::INFLIGHT, false);
1622 : }
1623 0 : break;
1624 :
1625 : #if AP_LANDINGGEAR_ENABLED
1626 0 : case AUX_FUNC::LANDING_GEAR: {
1627 0 : AP_LandingGear *lg = AP_LandingGear::get_singleton();
1628 0 : if (lg == nullptr) {
1629 0 : break;
1630 : }
1631 0 : switch (ch_flag) {
1632 0 : case AuxSwitchPos::LOW:
1633 0 : lg->set_position(AP_LandingGear::LandingGear_Deploy);
1634 0 : break;
1635 0 : case AuxSwitchPos::MIDDLE:
1636 : // nothing
1637 0 : break;
1638 0 : case AuxSwitchPos::HIGH:
1639 0 : lg->set_position(AP_LandingGear::LandingGear_Retract);
1640 0 : break;
1641 : }
1642 0 : break;
1643 : }
1644 : #endif
1645 :
1646 : #if AP_GPS_ENABLED
1647 6 : case AUX_FUNC::GPS_DISABLE:
1648 6 : AP::gps().force_disable(ch_flag == AuxSwitchPos::HIGH);
1649 : #if AP_EXTERNAL_AHRS_ENABLED
1650 6 : AP::externalAHRS().set_gnss_disable(ch_flag == AuxSwitchPos::HIGH);
1651 : #endif
1652 6 : break;
1653 :
1654 0 : case AUX_FUNC::GPS_DISABLE_YAW:
1655 0 : AP::gps().set_force_disable_yaw(ch_flag == AuxSwitchPos::HIGH);
1656 0 : break;
1657 : #endif // AP_GPS_ENABLED
1658 :
1659 : #if AP_AIRSPEED_ENABLED
1660 14 : case AUX_FUNC::DISABLE_AIRSPEED_USE: {
1661 14 : AP_Airspeed *airspeed = AP::airspeed();
1662 14 : if (airspeed == nullptr) {
1663 0 : break;
1664 : }
1665 14 : switch (ch_flag) {
1666 6 : case AuxSwitchPos::HIGH:
1667 6 : airspeed->force_disable_use(true);
1668 6 : break;
1669 1 : case AuxSwitchPos::MIDDLE:
1670 1 : break;
1671 7 : case AuxSwitchPos::LOW:
1672 7 : airspeed->force_disable_use(false);
1673 7 : break;
1674 : }
1675 14 : break;
1676 : }
1677 : #endif
1678 :
1679 7 : case AUX_FUNC::MOTOR_ESTOP:
1680 7 : switch (ch_flag) {
1681 5 : case AuxSwitchPos::HIGH: {
1682 5 : SRV_Channels::set_emergency_stop(true);
1683 5 : break;
1684 : }
1685 0 : case AuxSwitchPos::MIDDLE:
1686 : // nothing
1687 0 : break;
1688 2 : case AuxSwitchPos::LOW: {
1689 2 : SRV_Channels::set_emergency_stop(false);
1690 2 : break;
1691 : }
1692 : }
1693 7 : break;
1694 :
1695 : #if HAL_VISUALODOM_ENABLED
1696 2 : case AUX_FUNC::VISODOM_ALIGN:
1697 2 : if (ch_flag == AuxSwitchPos::HIGH) {
1698 1 : AP_VisualOdom *visual_odom = AP::visualodom();
1699 1 : if (visual_odom != nullptr) {
1700 1 : visual_odom->request_align_yaw_to_ahrs();
1701 : }
1702 : }
1703 2 : break;
1704 : #endif
1705 :
1706 : #if AP_AHRS_ENABLED
1707 12 : case AUX_FUNC::EKF_SOURCE_SET: {
1708 12 : AP_NavEKF_Source::SourceSetSelection source_set = AP_NavEKF_Source::SourceSetSelection::PRIMARY;
1709 12 : switch (ch_flag) {
1710 2 : case AuxSwitchPos::LOW:
1711 : // low switches to primary source
1712 2 : source_set = AP_NavEKF_Source::SourceSetSelection::PRIMARY;
1713 2 : break;
1714 7 : case AuxSwitchPos::MIDDLE:
1715 : // middle switches to secondary source
1716 7 : source_set = AP_NavEKF_Source::SourceSetSelection::SECONDARY;
1717 7 : break;
1718 3 : case AuxSwitchPos::HIGH:
1719 : // high switches to tertiary source
1720 3 : source_set = AP_NavEKF_Source::SourceSetSelection::TERTIARY;
1721 3 : break;
1722 : }
1723 12 : AP::ahrs().set_posvelyaw_source_set(source_set);
1724 12 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Using EKF Source Set %u", uint8_t(source_set)+1);
1725 12 : break;
1726 : }
1727 : #endif // AP_AHRS_ENABLED
1728 :
1729 : #if AP_OPTICALFLOW_CALIBRATOR_ENABLED
1730 5 : case AUX_FUNC::OPTFLOW_CAL: {
1731 5 : AP_OpticalFlow *optflow = AP::opticalflow();
1732 5 : if (optflow == nullptr) {
1733 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "OptFlow Cal: failed sensor not enabled");
1734 0 : break;
1735 : }
1736 5 : if (ch_flag == AuxSwitchPos::HIGH) {
1737 1 : optflow->start_calibration();
1738 : } else {
1739 4 : optflow->stop_calibration();
1740 : }
1741 5 : break;
1742 : }
1743 : #endif
1744 :
1745 : #if AP_INERTIALSENSOR_KILL_IMU_ENABLED
1746 0 : case AUX_FUNC::KILL_IMU1:
1747 0 : AP::ins().kill_imu(0, ch_flag == AuxSwitchPos::HIGH);
1748 0 : break;
1749 :
1750 0 : case AUX_FUNC::KILL_IMU2:
1751 0 : AP::ins().kill_imu(1, ch_flag == AuxSwitchPos::HIGH);
1752 0 : break;
1753 :
1754 0 : case AUX_FUNC::KILL_IMU3:
1755 0 : AP::ins().kill_imu(2, ch_flag == AuxSwitchPos::HIGH);
1756 0 : break;
1757 : #endif // AP_INERTIALSENSOR_KILL_IMU_ENABLED
1758 :
1759 : #if AP_CAMERA_ENABLED
1760 16 : case AUX_FUNC::CAMERA_TRIGGER:
1761 16 : do_aux_function_camera_trigger(ch_flag);
1762 16 : break;
1763 :
1764 0 : case AUX_FUNC::CAM_MODE_TOGGLE: {
1765 : // Momentary switch to for cycling camera modes
1766 0 : AP_Camera *camera = AP_Camera::get_singleton();
1767 0 : if (camera == nullptr) {
1768 0 : break;
1769 : }
1770 0 : switch (ch_flag) {
1771 0 : case AuxSwitchPos::LOW:
1772 : // nothing
1773 0 : break;
1774 0 : case AuxSwitchPos::MIDDLE:
1775 : // nothing
1776 0 : break;
1777 0 : case AuxSwitchPos::HIGH:
1778 0 : camera->cam_mode_toggle();
1779 0 : break;
1780 : }
1781 0 : break;
1782 : }
1783 0 : case AUX_FUNC::CAMERA_REC_VIDEO:
1784 0 : return do_aux_function_record_video(ch_flag);
1785 :
1786 0 : case AUX_FUNC::CAMERA_ZOOM:
1787 0 : return do_aux_function_camera_zoom(ch_flag);
1788 :
1789 0 : case AUX_FUNC::CAMERA_MANUAL_FOCUS:
1790 0 : return do_aux_function_camera_manual_focus(ch_flag);
1791 :
1792 0 : case AUX_FUNC::CAMERA_AUTO_FOCUS:
1793 0 : return do_aux_function_camera_auto_focus(ch_flag);
1794 :
1795 0 : case AUX_FUNC::CAMERA_IMAGE_TRACKING:
1796 0 : return do_aux_function_camera_image_tracking(ch_flag);
1797 :
1798 : #if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
1799 0 : case AUX_FUNC::CAMERA_LENS:
1800 0 : return do_aux_function_camera_lens(ch_flag);
1801 : #endif // AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
1802 : #endif // AP_CAMERA_ENABLED
1803 :
1804 : #if HAL_MOUNT_ENABLED
1805 0 : case AUX_FUNC::RETRACT_MOUNT1:
1806 0 : do_aux_function_retract_mount(ch_flag, 0);
1807 0 : break;
1808 :
1809 0 : case AUX_FUNC::RETRACT_MOUNT2:
1810 0 : do_aux_function_retract_mount(ch_flag, 1);
1811 0 : break;
1812 :
1813 0 : case AUX_FUNC::MOUNT_YAW_LOCK: {
1814 0 : AP_Mount *mount = AP::mount();
1815 0 : if (mount == nullptr) {
1816 0 : break;
1817 : }
1818 0 : mount->set_yaw_lock(ch_flag == AuxSwitchPos::HIGH);
1819 0 : break;
1820 : }
1821 :
1822 0 : case AUX_FUNC::MOUNT_RP_LOCK: {
1823 0 : AP_Mount *mount = AP::mount();
1824 0 : if (mount == nullptr) {
1825 0 : break;
1826 : }
1827 : //low is FPV:no ef locks,high is HORIZON lock:roll/pitch ef lock,middle is only pitch ef lock
1828 0 : switch (ch_flag) {
1829 0 : case AuxSwitchPos::HIGH:
1830 0 : mount->set_roll_lock(true);
1831 0 : mount->set_pitch_lock(true);
1832 0 : break;
1833 0 : case AuxSwitchPos::MIDDLE:
1834 0 : mount->set_roll_lock(false);
1835 0 : mount->set_pitch_lock(true);
1836 0 : break;
1837 0 : case AuxSwitchPos::LOW:
1838 0 : mount->set_roll_lock(false);
1839 0 : mount->set_pitch_lock(false);
1840 0 : break;
1841 : }
1842 0 : break;
1843 : }
1844 : #if AP_MOUNT_POI_LOCK_ENABLED
1845 13 : case AUX_FUNC::MOUNT_POI_LOCK: {
1846 13 : AP_Mount *mount = AP::mount();
1847 13 : if (mount == nullptr) {
1848 0 : break;
1849 : }
1850 13 : switch (ch_flag) {
1851 4 : case AuxSwitchPos::HIGH:
1852 4 : mount->set_poi_lock();
1853 4 : break;
1854 7 : case AuxSwitchPos::MIDDLE:
1855 7 : mount->suspend_poi_lock();
1856 7 : break;
1857 2 : case AuxSwitchPos::LOW:
1858 2 : mount->clear_poi_lock();
1859 2 : break;
1860 : }
1861 13 : break;
1862 : }
1863 : #endif // AP_MOUNT_POI_LOCK_ENABLED
1864 :
1865 0 : case AUX_FUNC::MOUNT_LRF_ENABLE: {
1866 0 : AP_Mount *mount = AP::mount();
1867 0 : if (mount == nullptr) {
1868 0 : break;
1869 : }
1870 0 : mount->set_rangefinder_enable(0, ch_flag == AuxSwitchPos::HIGH);
1871 0 : break;
1872 : }
1873 : #endif
1874 :
1875 : #if HAL_LOGGING_ENABLED
1876 0 : case AUX_FUNC::LOG_PAUSE: {
1877 0 : AP_Logger *logger = AP_Logger::get_singleton();
1878 0 : switch (ch_flag) {
1879 0 : case AuxSwitchPos::LOW:
1880 0 : logger->log_pause(false);
1881 0 : break;
1882 0 : case AuxSwitchPos::MIDDLE:
1883 : // nothing
1884 0 : break;
1885 0 : case AuxSwitchPos::HIGH:
1886 0 : logger->log_pause(true);
1887 0 : break;
1888 : }
1889 0 : break;
1890 : }
1891 : #endif
1892 :
1893 : #if COMPASS_CAL_ENABLED
1894 0 : case AUX_FUNC::MAG_CAL: {
1895 0 : Compass &compass = AP::compass();
1896 0 : switch (ch_flag) {
1897 0 : case AuxSwitchPos::LOW:
1898 0 : compass.cancel_calibration_all();
1899 0 : break;
1900 0 : case AuxSwitchPos::MIDDLE:
1901 : // nothing
1902 0 : break;
1903 0 : case AuxSwitchPos::HIGH:
1904 0 : if (!hal.util->get_soft_armed()) {
1905 0 : const bool retry = true;
1906 0 : const bool autosave = true;
1907 0 : const float delay = 5.0;
1908 0 : const bool autoreboot = false;
1909 0 : compass.start_calibration_all(retry, autosave, delay, autoreboot);
1910 : } else {
1911 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Disarm to allow compass calibration");
1912 : }
1913 0 : break;
1914 : }
1915 0 : break;
1916 : }
1917 : #endif
1918 :
1919 : #if AP_ARMING_ENABLED
1920 3 : case AUX_FUNC::ARM_EMERGENCY_STOP: {
1921 3 : switch (ch_flag) {
1922 1 : case AuxSwitchPos::HIGH:
1923 : // request arm, disable emergency motor stop
1924 1 : SRV_Channels::set_emergency_stop(false);
1925 1 : AP::arming().arm(AP_Arming::Method::AUXSWITCH, true);
1926 1 : break;
1927 0 : case AuxSwitchPos::MIDDLE:
1928 : // disable emergency motor stop
1929 0 : SRV_Channels::set_emergency_stop(false);
1930 0 : break;
1931 2 : case AuxSwitchPos::LOW:
1932 : // enable emergency motor stop
1933 2 : SRV_Channels::set_emergency_stop(true);
1934 2 : break;
1935 : }
1936 3 : break;
1937 : }
1938 : #endif // AP_ARMING_ENABLED
1939 :
1940 : #if AP_AHRS_ENABLED
1941 0 : case AUX_FUNC::EKF_LANE_SWITCH:
1942 : // used to test emergency lane switch
1943 0 : AP::ahrs().check_lane_switch();
1944 0 : break;
1945 :
1946 0 : case AUX_FUNC::EKF_YAW_RESET:
1947 : // used to test emergency yaw reset
1948 0 : AP::ahrs().request_yaw_reset();
1949 0 : break;
1950 :
1951 : #if AP_AHRS_EKF_RESET_ENABLED
1952 6 : case AUX_FUNC::EKF_RESET:
1953 6 : if (ch_flag == AuxSwitchPos::HIGH) {
1954 : // only allowed while disarmed - resetting the EKF in flight would
1955 : // discard the in-flight state estimate
1956 2 : if (hal.util->get_soft_armed()) {
1957 1 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF reset ignored: vehicle armed");
1958 1 : } else if (AP::ahrs().reset_configured_backend()) {
1959 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF bootstrap reset performed");
1960 : } else {
1961 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF bootstrap reset failed");
1962 : }
1963 : }
1964 6 : break;
1965 : #endif // AP_AHRS_EKF_RESET_ENABLED
1966 :
1967 0 : case AUX_FUNC::AHRS_TYPE: {
1968 : #if HAL_NAVEKF3_AVAILABLE && AP_EXTERNAL_AHRS_ENABLED
1969 0 : AP::ahrs().set_ekf_type(ch_flag==AuxSwitchPos::HIGH? AP_AHRS::EKFType::EXTERNAL : AP_AHRS::EKFType::THREE);
1970 : #endif
1971 0 : break;
1972 : }
1973 : #endif // AP_AHRS_ENABLED
1974 :
1975 : #if HAL_TORQEEDO_ENABLED
1976 : // clear torqeedo error
1977 0 : case AUX_FUNC::TORQEEDO_CLEAR_ERR: {
1978 0 : if (ch_flag == AuxSwitchPos::HIGH) {
1979 0 : AP_Torqeedo *torqeedo = AP_Torqeedo::get_singleton();
1980 0 : if (torqeedo != nullptr) {
1981 0 : torqeedo->clear_motor_error();
1982 : }
1983 : }
1984 0 : break;
1985 : }
1986 : #endif
1987 :
1988 : #if AP_SCRIPTING_ENABLED
1989 0 : case AUX_FUNC::STOP_RESTART_SCRIPTING: {
1990 0 : AP_Scripting *scr = AP::scripting();
1991 0 : if (scr != nullptr) {
1992 0 : switch (ch_flag) {
1993 0 : case AuxSwitchPos::HIGH:
1994 0 : scr->stop();
1995 0 : break;
1996 0 : case AuxSwitchPos::MIDDLE:
1997 0 : break;
1998 0 : case AuxSwitchPos::LOW:
1999 0 : scr->restart_all();
2000 0 : break;
2001 : }
2002 : }
2003 0 : break;
2004 : }
2005 : #endif
2006 :
2007 : // do nothing for these functions
2008 : #if HAL_MOUNT_ENABLED
2009 277 : case AUX_FUNC::MOUNT1_ROLL:
2010 : case AUX_FUNC::MOUNT1_PITCH:
2011 : case AUX_FUNC::MOUNT1_YAW:
2012 : case AUX_FUNC::MOUNT2_ROLL:
2013 : case AUX_FUNC::MOUNT2_PITCH:
2014 : case AUX_FUNC::MOUNT2_YAW:
2015 : #endif
2016 : #if AP_SCRIPTING_ENABLED
2017 : case AUX_FUNC::SCRIPTING_1:
2018 : case AUX_FUNC::SCRIPTING_2:
2019 : case AUX_FUNC::SCRIPTING_3:
2020 : case AUX_FUNC::SCRIPTING_4:
2021 : case AUX_FUNC::SCRIPTING_5:
2022 : case AUX_FUNC::SCRIPTING_6:
2023 : case AUX_FUNC::SCRIPTING_7:
2024 : case AUX_FUNC::SCRIPTING_8:
2025 : case AUX_FUNC::SCRIPTING_9:
2026 : case AUX_FUNC::SCRIPTING_10:
2027 : case AUX_FUNC::SCRIPTING_11:
2028 : case AUX_FUNC::SCRIPTING_12:
2029 : case AUX_FUNC::SCRIPTING_13:
2030 : case AUX_FUNC::SCRIPTING_14:
2031 : case AUX_FUNC::SCRIPTING_15:
2032 : case AUX_FUNC::SCRIPTING_16:
2033 : #endif
2034 277 : break;
2035 :
2036 : #if HAL_GENERATOR_ENABLED
2037 25 : case AUX_FUNC::LOWEHEISER_THROTTLE:
2038 : case AUX_FUNC::LOWEHEISER_STARTER:
2039 : // monitored by the library itself
2040 25 : break;
2041 : #endif
2042 :
2043 4 : default:
2044 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Invalid channel option (%u)", (unsigned int)ch_option);
2045 4 : return false;
2046 : }
2047 :
2048 614 : return true;
2049 : }
2050 :
2051 33456 : void RC_Channel::init_aux()
2052 : {
2053 : AuxSwitchPos position;
2054 33456 : if (!read_3pos_switch(position)) {
2055 33456 : position = AuxSwitchPos::LOW;
2056 : }
2057 :
2058 33456 : init_aux_function((AUX_FUNC)option.get(), position);
2059 33456 : }
2060 :
2061 : // read_3pos_switch
2062 1041532 : bool RC_Channel::read_3pos_switch(RC_Channel::AuxSwitchPos &ret) const
2063 : {
2064 1041532 : const uint16_t in = get_radio_in();
2065 1041532 : if (in <= RC_MIN_LIMIT_PWM || in >= RC_MAX_LIMIT_PWM) {
2066 34170 : return false;
2067 : }
2068 :
2069 : // switch is reversed if 'reversed' option set on channel and switches reverse is allowed by RC_OPTIONS
2070 1007362 : bool switch_reversed = reversed && rc().option_is_enabled(RC_Channels::Option::ALLOW_SWITCH_REV);
2071 :
2072 1007362 : if (in < AUX_SWITCH_PWM_TRIGGER_LOW) {
2073 102256 : ret = switch_reversed ? AuxSwitchPos::HIGH : AuxSwitchPos::LOW;
2074 905106 : } else if (in > AUX_SWITCH_PWM_TRIGGER_HIGH) {
2075 112606 : ret = switch_reversed ? AuxSwitchPos::LOW : AuxSwitchPos::HIGH;
2076 : } else {
2077 792500 : ret = AuxSwitchPos::MIDDLE;
2078 : }
2079 1007362 : return true;
2080 : }
2081 :
2082 : // return switch position value as LOW, MIDDLE, HIGH
2083 : // if reading the switch fails then it returns LOW
2084 321 : RC_Channel::AuxSwitchPos RC_Channel::get_aux_switch_pos() const
2085 : {
2086 321 : AuxSwitchPos position = AuxSwitchPos::LOW;
2087 321 : UNUSED_RESULT(read_3pos_switch(position));
2088 :
2089 321 : return position;
2090 : }
2091 :
2092 : // return stick gesture pos as LOW, MIDDLE, HIGH
2093 : // this function uses different threshold values to RC_Chanel::get_aux_switch_pos()
2094 : // to avoid glitching on the stick travel and also always honours channel reversal
2095 0 : RC_Channel::AuxSwitchPos RC_Channel::get_stick_gesture_pos() const
2096 : {
2097 0 : const uint16_t in = get_radio_in();
2098 0 : if (in <= 900 || in >= 2200) {
2099 0 : return RC_Channel::AuxSwitchPos::LOW;
2100 : }
2101 :
2102 : // switch is reversed if 'reversed' option set on channel and switches reverse is allowed by RC_OPTIONS
2103 0 : bool switch_reversed = get_reverse();
2104 :
2105 0 : if (in < RC_Channel::AUX_PWM_TRIGGER_LOW) {
2106 0 : return switch_reversed ? RC_Channel::AuxSwitchPos::HIGH : RC_Channel::AuxSwitchPos::LOW;
2107 : }
2108 0 : if (in > RC_Channel::AUX_PWM_TRIGGER_HIGH) {
2109 0 : return switch_reversed ? RC_Channel::AuxSwitchPos::LOW : RC_Channel::AuxSwitchPos::HIGH;
2110 : }
2111 0 : return RC_Channel::AuxSwitchPos::MIDDLE;
2112 : }
2113 :
2114 4610257 : RC_Channel *RC_Channels::find_channel_for_option(const RC_Channel::AUX_FUNC option)
2115 : {
2116 66491492 : for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
2117 63128299 : RC_Channel *c = channel(i);
2118 63128299 : if (c == nullptr) {
2119 : // odd?
2120 0 : continue;
2121 : }
2122 63128299 : if ((RC_Channel::AUX_FUNC)c->option.get() == option) {
2123 1247064 : return c;
2124 : }
2125 : }
2126 3363193 : return nullptr;
2127 : }
2128 :
2129 : // duplicate_options_exist - returns true if any options are duplicated
2130 1028 : bool RC_Channels::duplicate_options_exist()
2131 : {
2132 1028 : Bitmask<(uint16_t)RC_Channel::AUX_FUNC::AUX_FUNCTION_MAX> used_auxsw_options;
2133 17466 : for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
2134 16439 : const RC_Channel *c = channel(i);
2135 16439 : if (c == nullptr) {
2136 : // odd?
2137 0 : continue;
2138 : }
2139 16439 : const uint16_t option = c->option.get();
2140 16439 : if (option == (uint16_t)RC_Channel::AUX_FUNC::DO_NOTHING) {
2141 15628 : continue;
2142 : }
2143 811 : if (option >= used_auxsw_options.size()) {
2144 0 : continue;
2145 : }
2146 811 : if (used_auxsw_options.get(option)) {
2147 1 : return true;
2148 : }
2149 810 : used_auxsw_options.set(option);
2150 : }
2151 1027 : return false;
2152 : }
2153 :
2154 : #endif // AP_RC_CHANNEL_ENABLED
|
