Line data Source code
1 : #include "Copter.h"
2 :
3 : #include "RC_Channel_Copter.h"
4 :
5 :
6 : // defining these two macros and including the RC_Channels_VarInfo header defines the parameter information common to all vehicle types
7 : #define RC_CHANNELS_SUBCLASS RC_Channels_Copter
8 : #define RC_CHANNEL_SUBCLASS RC_Channel_Copter
9 :
10 : #include <RC_Channel/RC_Channels_VarInfo.h>
11 :
12 29983618 : int8_t RC_Channels_Copter::flight_mode_channel_number() const
13 : {
14 29983618 : return copter.g.flight_mode_chan.get();
15 : }
16 :
17 1029 : void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos)
18 : {
19 1029 : if (new_pos < 0 || (uint8_t)new_pos >= ARRAY_SIZE(copter.g.flight_modes)) {
20 : // should not have been called
21 0 : return;
22 : }
23 :
24 1029 : if (!copter.set_mode((Mode::Number)copter.g.flight_modes[new_pos].get(), ModeReason::RC_COMMAND)) {
25 0 : return;
26 : }
27 :
28 2058 : if (!rc().find_channel_for_option(AUX_FUNC::SIMPLE_MODE) &&
29 1029 : !rc().find_channel_for_option(AUX_FUNC::SUPERSIMPLE_MODE)) {
30 : // if none of the Aux Switches are set to Simple or Super Simple Mode then
31 : // set Simple Mode using stored parameters from EEPROM
32 1029 : if (BIT_IS_SET(copter.g.super_simple, new_pos)) {
33 0 : copter.set_simple_mode(Copter::SimpleMode::SUPERSIMPLE);
34 : } else {
35 1029 : copter.set_simple_mode(BIT_IS_SET(copter.g.simple_modes, new_pos) ? Copter::SimpleMode::SIMPLE : Copter::SimpleMode::NONE);
36 : }
37 : }
38 : }
39 :
40 104678133 : bool RC_Channels_Copter::in_rc_failsafe() const
41 : {
42 104678133 : return copter.failsafe.radio;
43 : }
44 :
45 103524926 : bool RC_Channels_Copter::has_valid_input() const
46 : {
47 103524926 : if (in_rc_failsafe()) {
48 1000776 : return false;
49 : }
50 102524150 : if (copter.failsafe.radio_counter != 0) {
51 374 : return false;
52 : }
53 102523776 : return RC_Channels::has_valid_input();
54 : }
55 :
56 : // returns true if throttle arming checks should be run
57 25316 : bool RC_Channels_Copter::arming_check_throttle() const {
58 25316 : if ((copter.g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {
59 : // center sprung throttle configured, dont run AP_Arming check
60 : // Copter already checks this case in its own arming checks
61 305 : return false;
62 : }
63 25011 : return RC_Channels::arming_check_throttle();
64 : }
65 :
66 3738105 : RC_Channel * RC_Channels_Copter::get_arming_channel(void) const
67 : {
68 3738105 : return copter.channel_yaw;
69 : }
70 :
71 : // init_aux_switch_function - initialize aux functions
72 16016 : void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag)
73 : {
74 : // init channel options
75 16016 : switch(ch_option) {
76 : // the following functions do not need to be initialised:
77 905 : case AUX_FUNC::ALTHOLD:
78 : case AUX_FUNC::AUTO:
79 : case AUX_FUNC::AUTOTUNE_MODE:
80 : case AUX_FUNC::AUTOTUNE_TEST_GAINS:
81 : case AUX_FUNC::BRAKE:
82 : case AUX_FUNC::CIRCLE:
83 : case AUX_FUNC::DRIFT:
84 : case AUX_FUNC::FLIP:
85 : case AUX_FUNC::FLOWHOLD:
86 : case AUX_FUNC::FOLLOW:
87 : case AUX_FUNC::GUIDED:
88 : case AUX_FUNC::LAND:
89 : case AUX_FUNC::LOITER:
90 : #if HAL_PARACHUTE_ENABLED
91 : case AUX_FUNC::PARACHUTE_RELEASE:
92 : #endif
93 : case AUX_FUNC::POSHOLD:
94 : case AUX_FUNC::RESETTOARMEDYAW:
95 : case AUX_FUNC::RTL:
96 : case AUX_FUNC::SAVE_TRIM:
97 : #if MODE_AUTO_ENABLED
98 : case AUX_FUNC::SAVE_WP:
99 : #endif // MODE_AUTO_ENABLED
100 : case AUX_FUNC::SMART_RTL:
101 : case AUX_FUNC::STABILIZE:
102 : case AUX_FUNC::THROW:
103 : case AUX_FUNC::USER_FUNC1:
104 : case AUX_FUNC::USER_FUNC2:
105 : case AUX_FUNC::USER_FUNC3:
106 : #if AP_WINCH_ENABLED
107 : case AUX_FUNC::WINCH_CONTROL:
108 : #endif
109 : case AUX_FUNC::ZIGZAG:
110 : case AUX_FUNC::ZIGZAG_Auto:
111 : case AUX_FUNC::ZIGZAG_SaveWP:
112 : case AUX_FUNC::ACRO:
113 : #if MODE_AUTO_ENABLED
114 : case AUX_FUNC::AUTO_RTL:
115 : #endif // MODE_AUTO_ENABLED
116 : case AUX_FUNC::TURTLE:
117 : case AUX_FUNC::SIMPLE_HEADING_RESET:
118 : case AUX_FUNC::ARMDISARM_AIRMODE:
119 : case AUX_FUNC::TURBINE_START:
120 : case AUX_FUNC::FLIGHTMODE_PAUSE:
121 : #if AP_COPTER_AHRS_AUTO_TRIM_ENABLED
122 : case AUX_FUNC::AHRS_AUTO_TRIM:
123 : #endif
124 905 : break;
125 109 : case AUX_FUNC::ACRO_TRAINER:
126 : case AUX_FUNC::ATTCON_ACCEL_LIM:
127 : case AUX_FUNC::ATTCON_FEEDFWD:
128 : case AUX_FUNC::INVERTED:
129 : case AUX_FUNC::MOTOR_INTERLOCK:
130 : #if HAL_PARACHUTE_ENABLED
131 : case AUX_FUNC::PARACHUTE_3POS: // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release
132 : case AUX_FUNC::PARACHUTE_ENABLE:
133 : #endif
134 : case AUX_FUNC::PRECISION_LOITER:
135 : #if AP_RANGEFINDER_ENABLED
136 : case AUX_FUNC::RANGEFINDER:
137 : #endif
138 : case AUX_FUNC::SIMPLE_MODE:
139 : case AUX_FUNC::STANDBY:
140 : case AUX_FUNC::SUPERSIMPLE_MODE:
141 : case AUX_FUNC::SURFACE_TRACKING:
142 : #if AP_WINCH_ENABLED
143 : case AUX_FUNC::WINCH_ENABLE:
144 : #endif
145 : case AUX_FUNC::AIRMODE:
146 : case AUX_FUNC::FORCEFLYING:
147 : case AUX_FUNC::CUSTOM_CONTROLLER:
148 : case AUX_FUNC::WEATHER_VANE_ENABLE:
149 : #if AP_RC_TRANSMITTER_TUNING_ENABLED
150 : case AUX_FUNC::TRANSMITTER_TUNING:
151 : case AUX_FUNC::TRANSMITTER_TUNING2:
152 109 : run_aux_function(ch_option, ch_flag, AuxFuncTrigger::Source::INIT, ch_in);
153 109 : break;
154 : #endif // AP_RC_TRANSMITTER_TUNING_ENABLED
155 15002 : default:
156 15002 : RC_Channel::init_aux_function(ch_option, ch_flag);
157 15002 : break;
158 : }
159 16016 : }
160 :
161 : // do_aux_function_change_mode - change mode based on an aux switch
162 : // being moved
163 10 : void RC_Channel_Copter::do_aux_function_change_mode(const Mode::Number mode,
164 : const AuxSwitchPos ch_flag)
165 : {
166 10 : switch(ch_flag) {
167 6 : case AuxSwitchPos::HIGH: {
168 : // engage mode (if not possible we remain in current flight mode)
169 6 : copter.set_mode(mode, ModeReason::AUX_FUNCTION);
170 6 : break;
171 : }
172 4 : default:
173 : // return to flight mode switch's flight mode if we are currently
174 : // in this mode
175 4 : if (copter.flightmode->mode_number() == mode) {
176 2 : rc().reset_mode_switch();
177 : }
178 : }
179 10 : }
180 :
181 : // do_aux_function - implement the function invoked by auxiliary switches
182 1657 : bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
183 : {
184 1657 : const AUX_FUNC &ch_option = trigger.func;
185 1657 : const AuxSwitchPos &ch_flag = trigger.pos;
186 :
187 1657 : switch(ch_option) {
188 8 : case AUX_FUNC::FLIP:
189 8 : if (ch_flag == AuxSwitchPos::HIGH) {
190 4 : copter.set_mode(Mode::Number::FLIP, ModeReason::AUX_FUNCTION);
191 : } else {
192 : #if MODE_FLIP_ENABLED
193 4 : copter.mode_flip.abandon_flip();
194 : #endif
195 : }
196 8 : break;
197 :
198 0 : case AUX_FUNC::SIMPLE_MODE:
199 : // low = simple mode off, middle or high position turns simple mode on
200 0 : copter.set_simple_mode((ch_flag == AuxSwitchPos::LOW) ? Copter::SimpleMode::NONE : Copter::SimpleMode::SIMPLE);
201 0 : break;
202 :
203 0 : case AUX_FUNC::SUPERSIMPLE_MODE: {
204 0 : Copter::SimpleMode newmode = Copter::SimpleMode::NONE;
205 0 : switch (ch_flag) {
206 0 : case AuxSwitchPos::LOW:
207 0 : break;
208 0 : case AuxSwitchPos::MIDDLE:
209 0 : newmode = Copter::SimpleMode::SIMPLE;
210 0 : break;
211 0 : case AuxSwitchPos::HIGH:
212 0 : newmode = Copter::SimpleMode::SUPERSIMPLE;
213 0 : break;
214 : }
215 0 : copter.set_simple_mode(newmode);
216 0 : break;
217 : }
218 :
219 : #if MODE_RTL_ENABLED
220 0 : case AUX_FUNC::RTL:
221 0 : do_aux_function_change_mode(Mode::Number::RTL, ch_flag);
222 0 : break;
223 : #endif
224 :
225 0 : case AUX_FUNC::SAVE_TRIM:
226 0 : if ((ch_flag == AuxSwitchPos::HIGH) &&
227 0 : (copter.flightmode->allows_save_trim()) &&
228 0 : (copter.channel_throttle->get_control_in() == 0)) {
229 0 : copter.g2.rc_channels.save_trim();
230 : }
231 0 : break;
232 :
233 : #if MODE_AUTO_ENABLED
234 915 : case AUX_FUNC::SAVE_WP:
235 : // save waypoint when switch is brought high
236 915 : if (ch_flag == RC_Channel::AuxSwitchPos::HIGH) {
237 :
238 : // do not allow saving new waypoints while we're in auto or disarmed
239 11 : if (copter.flightmode == &copter.mode_auto || !copter.motors->armed()) {
240 5 : break;
241 : }
242 :
243 : // do not allow saving the first waypoint with zero throttle
244 6 : if (!copter.mode_auto.mission.present() && (copter.channel_throttle->get_control_in() == 0)) {
245 0 : break;
246 : }
247 :
248 : // create new mission command
249 6 : AP_Mission::Mission_Command cmd = {};
250 :
251 : // if the mission is empty save a takeoff command
252 6 : if (!copter.mode_auto.mission.present()) {
253 : // set our location ID to 16, MAV_CMD_NAV_WAYPOINT
254 1 : cmd.id = MAV_CMD_NAV_TAKEOFF;
255 1 : cmd.content.location.alt = MAX(copter.current_loc.alt,100);
256 :
257 : // use the current altitude for the target alt for takeoff.
258 : // only altitude will matter to the AP mission script for takeoff.
259 1 : if (copter.mode_auto.mission.add_cmd(cmd)) {
260 : // log event
261 1 : LOGGER_WRITE_EVENT(LogEvent::SAVEWP_ADD_WP);
262 : }
263 : }
264 :
265 : // set new waypoint to current location
266 6 : cmd.content.location = copter.current_loc;
267 :
268 : // if throttle is above zero, create waypoint command
269 6 : if (copter.channel_throttle->get_control_in() > 0) {
270 6 : cmd.id = MAV_CMD_NAV_WAYPOINT;
271 : } else {
272 : // with zero throttle, create LAND command
273 0 : cmd.id = MAV_CMD_NAV_LAND;
274 : }
275 :
276 : // save command
277 6 : if (copter.mode_auto.mission.add_cmd(cmd)) {
278 : // log event
279 6 : LOGGER_WRITE_EVENT(LogEvent::SAVEWP_ADD_WP);
280 : }
281 : }
282 910 : break;
283 :
284 0 : case AUX_FUNC::AUTO:
285 0 : do_aux_function_change_mode(Mode::Number::AUTO, ch_flag);
286 0 : break;
287 : #endif
288 :
289 : #if AP_RANGEFINDER_ENABLED
290 8 : case AUX_FUNC::RANGEFINDER:
291 : // enable or disable the rangefinder
292 13 : if ((ch_flag == AuxSwitchPos::HIGH) &&
293 5 : copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
294 3 : copter.rangefinder_state.enabled = true;
295 : } else {
296 5 : copter.rangefinder_state.enabled = false;
297 : }
298 8 : break;
299 : #endif // AP_RANGEFINDER_ENABLED
300 :
301 : #if MODE_ACRO_ENABLED
302 0 : case AUX_FUNC::ACRO_TRAINER:
303 0 : switch(ch_flag) {
304 0 : case AuxSwitchPos::LOW:
305 0 : copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::OFF);
306 0 : LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_OFF);
307 0 : break;
308 0 : case AuxSwitchPos::MIDDLE:
309 0 : copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::LEVELING);
310 0 : LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_LEVELING);
311 0 : break;
312 0 : case AuxSwitchPos::HIGH:
313 0 : copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::LIMITED);
314 0 : LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_LIMITED);
315 0 : break;
316 : }
317 0 : break;
318 : #endif
319 :
320 : #if AUTOTUNE_ENABLED
321 5 : case AUX_FUNC::AUTOTUNE_MODE:
322 5 : do_aux_function_change_mode(Mode::Number::AUTOTUNE, ch_flag);
323 5 : break;
324 5 : case AUX_FUNC::AUTOTUNE_TEST_GAINS:
325 5 : copter.mode_autotune.autotune.do_aux_function(ch_flag);
326 5 : break;
327 : #endif
328 :
329 1 : case AUX_FUNC::LAND:
330 1 : do_aux_function_change_mode(Mode::Number::LAND, ch_flag);
331 1 : break;
332 :
333 3 : case AUX_FUNC::GUIDED:
334 3 : do_aux_function_change_mode(Mode::Number::GUIDED, ch_flag);
335 3 : break;
336 :
337 0 : case AUX_FUNC::LOITER:
338 0 : do_aux_function_change_mode(Mode::Number::LOITER, ch_flag);
339 0 : break;
340 :
341 0 : case AUX_FUNC::FOLLOW:
342 0 : do_aux_function_change_mode(Mode::Number::FOLLOW, ch_flag);
343 0 : break;
344 :
345 : #if HAL_PARACHUTE_ENABLED
346 0 : case AUX_FUNC::PARACHUTE_ENABLE:
347 : // Parachute enable/disable
348 0 : copter.parachute.enabled(ch_flag == AuxSwitchPos::HIGH);
349 0 : break;
350 :
351 0 : case AUX_FUNC::PARACHUTE_RELEASE:
352 0 : if (ch_flag == AuxSwitchPos::HIGH) {
353 0 : copter.parachute_manual_release();
354 : }
355 0 : break;
356 :
357 38 : case AUX_FUNC::PARACHUTE_3POS:
358 : // Parachute disable, enable, release with 3 position switch
359 38 : switch (ch_flag) {
360 34 : case AuxSwitchPos::LOW:
361 34 : copter.parachute.enabled(false);
362 34 : break;
363 2 : case AuxSwitchPos::MIDDLE:
364 2 : copter.parachute.enabled(true);
365 2 : break;
366 2 : case AuxSwitchPos::HIGH:
367 2 : copter.parachute.enabled(true);
368 2 : copter.parachute_manual_release();
369 2 : break;
370 : }
371 38 : break;
372 : #endif // HAL_PARACHUTE_ENABLED
373 :
374 0 : case AUX_FUNC::ATTCON_FEEDFWD:
375 : // enable or disable feed forward
376 0 : copter.attitude_control->bf_feedforward(ch_flag == AuxSwitchPos::HIGH);
377 0 : break;
378 :
379 0 : case AUX_FUNC::ATTCON_ACCEL_LIM:
380 : // enable or disable accel limiting by restoring defaults
381 0 : copter.attitude_control->accel_limiting(ch_flag == AuxSwitchPos::HIGH);
382 0 : break;
383 :
384 255 : case AUX_FUNC::MOTOR_INTERLOCK:
385 : #if FRAME_CONFIG == HELI_FRAME
386 : // The interlock logic for ROTOR_CONTROL_MODE_PASSTHROUGH is handled
387 : // in heli_update_rotor_speed_targets. Otherwise turn on when above low.
388 252 : if (copter.motors->get_rsc_mode() != ROTOR_CONTROL_MODE_PASSTHROUGH) {
389 252 : copter.ap.motor_interlock_switch = (ch_flag == AuxSwitchPos::HIGH || ch_flag == AuxSwitchPos::MIDDLE);
390 : }
391 : #else
392 3 : copter.ap.motor_interlock_switch = (ch_flag == AuxSwitchPos::HIGH || ch_flag == AuxSwitchPos::MIDDLE);
393 : #endif
394 255 : break;
395 :
396 : #if FRAME_CONFIG == HELI_FRAME
397 12 : case AUX_FUNC::TURBINE_START:
398 12 : switch (ch_flag) {
399 5 : case AuxSwitchPos::HIGH:
400 5 : copter.motors->set_turb_start(true);
401 5 : break;
402 2 : case AuxSwitchPos::MIDDLE:
403 : // nothing
404 2 : break;
405 5 : case AuxSwitchPos::LOW:
406 5 : copter.motors->set_turb_start(false);
407 5 : break;
408 : }
409 12 : break;
410 : #endif
411 :
412 : #if MODE_BRAKE_ENABLED
413 0 : case AUX_FUNC::BRAKE:
414 0 : do_aux_function_change_mode(Mode::Number::BRAKE, ch_flag);
415 0 : break;
416 : #endif
417 :
418 : #if MODE_THROW_ENABLED
419 0 : case AUX_FUNC::THROW:
420 0 : do_aux_function_change_mode(Mode::Number::THROW, ch_flag);
421 0 : break;
422 : #endif
423 :
424 : #if AC_PRECLAND_ENABLED && MODE_LOITER_ENABLED
425 5 : case AUX_FUNC::PRECISION_LOITER:
426 5 : switch (ch_flag) {
427 2 : case AuxSwitchPos::HIGH:
428 2 : copter.mode_loiter.set_precision_loiter_enabled(true);
429 2 : break;
430 1 : case AuxSwitchPos::MIDDLE:
431 : // nothing
432 1 : break;
433 2 : case AuxSwitchPos::LOW:
434 2 : copter.mode_loiter.set_precision_loiter_enabled(false);
435 2 : break;
436 : }
437 5 : break;
438 : #endif
439 :
440 : #if MODE_SMARTRTL_ENABLED
441 0 : case AUX_FUNC::SMART_RTL:
442 0 : do_aux_function_change_mode(Mode::Number::SMART_RTL, ch_flag);
443 0 : break;
444 : #endif
445 :
446 : #if FRAME_CONFIG == HELI_FRAME
447 6 : case AUX_FUNC::INVERTED:
448 6 : switch (ch_flag) {
449 2 : case AuxSwitchPos::HIGH:
450 2 : if (copter.flightmode->allows_inverted()) {
451 2 : copter.attitude_control->set_inverted_flight(true);
452 : } else {
453 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Inverted flight not available in %s mode", copter.flightmode->name());
454 : }
455 2 : break;
456 0 : case AuxSwitchPos::MIDDLE:
457 : // nothing
458 0 : break;
459 4 : case AuxSwitchPos::LOW:
460 4 : copter.attitude_control->set_inverted_flight(false);
461 4 : break;
462 : }
463 6 : break;
464 : #endif
465 :
466 : #if AP_WINCH_ENABLED
467 0 : case AUX_FUNC::WINCH_ENABLE:
468 0 : switch (ch_flag) {
469 0 : case AuxSwitchPos::HIGH:
470 : // high switch position stops winch using rate control
471 0 : copter.g2.winch.set_desired_rate(0.0f);
472 0 : break;
473 0 : case AuxSwitchPos::MIDDLE:
474 : case AuxSwitchPos::LOW:
475 : // all other position relax winch
476 0 : copter.g2.winch.relax();
477 0 : break;
478 : }
479 0 : break;
480 :
481 4 : case AUX_FUNC::WINCH_CONTROL:
482 : // do nothing, used to control the rate of the winch and is processed within AP_Winch
483 4 : break;
484 : #endif // AP_WINCH_ENABLED
485 :
486 : #ifdef USERHOOK_AUXSWITCH
487 : case AUX_FUNC::USER_FUNC1:
488 : copter.userhook_auxSwitch1(ch_flag);
489 : break;
490 :
491 : case AUX_FUNC::USER_FUNC2:
492 : copter.userhook_auxSwitch2(ch_flag);
493 : break;
494 :
495 : case AUX_FUNC::USER_FUNC3:
496 : copter.userhook_auxSwitch3(ch_flag);
497 : break;
498 : #endif
499 :
500 : #if MODE_ZIGZAG_ENABLED
501 0 : case AUX_FUNC::ZIGZAG:
502 0 : do_aux_function_change_mode(Mode::Number::ZIGZAG, ch_flag);
503 0 : break;
504 :
505 56 : case AUX_FUNC::ZIGZAG_SaveWP:
506 56 : if (copter.flightmode == &copter.mode_zigzag) {
507 : // initialize zigzag auto
508 52 : copter.mode_zigzag.init_auto();
509 52 : switch (ch_flag) {
510 16 : case AuxSwitchPos::LOW:
511 16 : copter.mode_zigzag.save_or_move_to_destination(ModeZigZag::Destination::A);
512 16 : break;
513 24 : case AuxSwitchPos::MIDDLE:
514 24 : copter.mode_zigzag.return_to_manual_control(false);
515 24 : break;
516 12 : case AuxSwitchPos::HIGH:
517 12 : copter.mode_zigzag.save_or_move_to_destination(ModeZigZag::Destination::B);
518 12 : break;
519 : }
520 : }
521 56 : break;
522 : #endif
523 :
524 0 : case AUX_FUNC::STABILIZE:
525 0 : do_aux_function_change_mode(Mode::Number::STABILIZE, ch_flag);
526 0 : break;
527 :
528 : #if MODE_POSHOLD_ENABLED
529 0 : case AUX_FUNC::POSHOLD:
530 0 : do_aux_function_change_mode(Mode::Number::POSHOLD, ch_flag);
531 0 : break;
532 : #endif
533 :
534 : #if MODE_ALTHOLD_ENABLED
535 1 : case AUX_FUNC::ALTHOLD:
536 1 : do_aux_function_change_mode(Mode::Number::ALT_HOLD, ch_flag);
537 1 : break;
538 : #endif
539 :
540 : #if MODE_ACRO_ENABLED
541 0 : case AUX_FUNC::ACRO:
542 0 : do_aux_function_change_mode(Mode::Number::ACRO, ch_flag);
543 0 : break;
544 : #endif
545 :
546 : #if MODE_FLOWHOLD_ENABLED
547 0 : case AUX_FUNC::FLOWHOLD:
548 0 : do_aux_function_change_mode(Mode::Number::FLOWHOLD, ch_flag);
549 0 : break;
550 : #endif
551 :
552 : #if MODE_CIRCLE_ENABLED
553 0 : case AUX_FUNC::CIRCLE:
554 0 : do_aux_function_change_mode(Mode::Number::CIRCLE, ch_flag);
555 0 : break;
556 : #endif
557 :
558 : #if MODE_DRIFT_ENABLED
559 0 : case AUX_FUNC::DRIFT:
560 0 : do_aux_function_change_mode(Mode::Number::DRIFT, ch_flag);
561 0 : break;
562 : #endif
563 :
564 0 : case AUX_FUNC::STANDBY: {
565 0 : switch (ch_flag) {
566 0 : case AuxSwitchPos::HIGH:
567 0 : copter.standby_active = true;
568 0 : LOGGER_WRITE_EVENT(LogEvent::STANDBY_ENABLE);
569 0 : gcs().send_text(MAV_SEVERITY_INFO, "Stand By Enabled");
570 0 : break;
571 0 : default:
572 0 : copter.standby_active = false;
573 0 : LOGGER_WRITE_EVENT(LogEvent::STANDBY_DISABLE);
574 0 : gcs().send_text(MAV_SEVERITY_INFO, "Stand By Disabled");
575 0 : break;
576 : }
577 0 : break;
578 : }
579 :
580 : #if AP_RANGEFINDER_ENABLED
581 0 : case AUX_FUNC::SURFACE_TRACKING:
582 0 : switch (ch_flag) {
583 0 : case AuxSwitchPos::LOW:
584 0 : copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::GROUND);
585 0 : break;
586 0 : case AuxSwitchPos::MIDDLE:
587 0 : copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::NONE);
588 0 : break;
589 0 : case AuxSwitchPos::HIGH:
590 0 : copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::CEILING);
591 0 : break;
592 : }
593 0 : break;
594 : #endif
595 :
596 0 : case AUX_FUNC::FLIGHTMODE_PAUSE:
597 0 : switch (ch_flag) {
598 0 : case AuxSwitchPos::HIGH:
599 0 : if (!copter.flightmode->pause()) {
600 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Flight Mode Pause failed");
601 : }
602 0 : break;
603 0 : case AuxSwitchPos::MIDDLE:
604 0 : break;
605 0 : case AuxSwitchPos::LOW:
606 0 : copter.flightmode->resume();
607 0 : break;
608 : }
609 0 : break;
610 :
611 : #if MODE_ZIGZAG_ENABLED
612 0 : case AUX_FUNC::ZIGZAG_Auto:
613 0 : if (copter.flightmode == &copter.mode_zigzag) {
614 0 : switch (ch_flag) {
615 0 : case AuxSwitchPos::HIGH:
616 0 : copter.mode_zigzag.run_auto();
617 0 : break;
618 0 : default:
619 0 : copter.mode_zigzag.suspend_auto();
620 0 : break;
621 : }
622 : }
623 0 : break;
624 : #endif
625 :
626 15 : case AUX_FUNC::AIRMODE:
627 15 : do_aux_function_change_air_mode(ch_flag);
628 : #if MODE_ACRO_ENABLED && FRAME_CONFIG != HELI_FRAME
629 15 : copter.mode_acro.air_mode_aux_changed();
630 : #endif
631 15 : break;
632 :
633 0 : case AUX_FUNC::FORCEFLYING:
634 0 : do_aux_function_change_force_flying(ch_flag);
635 0 : break;
636 :
637 : #if MODE_AUTO_ENABLED
638 0 : case AUX_FUNC::AUTO_RTL:
639 0 : do_aux_function_change_mode(Mode::Number::AUTO_RTL, ch_flag);
640 0 : break;
641 : #endif
642 :
643 : #if MODE_TURTLE_ENABLED
644 0 : case AUX_FUNC::TURTLE:
645 0 : do_aux_function_change_mode(Mode::Number::TURTLE, ch_flag);
646 0 : break;
647 : #endif
648 :
649 : #if AP_COPTER_AHRS_AUTO_TRIM_ENABLED
650 12 : case AUX_FUNC::AHRS_AUTO_TRIM:
651 12 : copter.g2.rc_channels.do_aux_function_ahrs_auto_trim(ch_flag);
652 12 : break;
653 : #endif // AP_COPTER_AHRS_AUTO_TRIM_ENABLED
654 :
655 0 : case AUX_FUNC::SIMPLE_HEADING_RESET:
656 0 : if (ch_flag == AuxSwitchPos::HIGH) {
657 0 : copter.init_simple_bearing();
658 0 : gcs().send_text(MAV_SEVERITY_INFO, "Simple heading reset");
659 : }
660 0 : break;
661 :
662 4 : case AUX_FUNC::ARMDISARM_AIRMODE:
663 4 : RC_Channel::do_aux_function_armdisarm(ch_flag);
664 4 : if (copter.arming.is_armed()) {
665 2 : copter.ap.armed_with_airmode_switch = true;
666 : }
667 4 : break;
668 :
669 : #if AC_CUSTOMCONTROL_MULTI_ENABLED
670 6 : case AUX_FUNC::CUSTOM_CONTROLLER:
671 6 : copter.custom_control.set_custom_controller(ch_flag == AuxSwitchPos::HIGH);
672 6 : break;
673 : #endif
674 :
675 : #if WEATHERVANE_ENABLED
676 0 : case AUX_FUNC::WEATHER_VANE_ENABLE: {
677 0 : switch (ch_flag) {
678 0 : case AuxSwitchPos::HIGH:
679 0 : copter.g2.weathervane.allow_weathervaning(true);
680 0 : break;
681 0 : case AuxSwitchPos::MIDDLE:
682 0 : break;
683 0 : case AuxSwitchPos::LOW:
684 0 : copter.g2.weathervane.allow_weathervaning(false);
685 0 : break;
686 : }
687 0 : break;
688 : }
689 : #endif
690 : #if AP_RC_TRANSMITTER_TUNING_ENABLED
691 25 : case AUX_FUNC::TRANSMITTER_TUNING:
692 : case AUX_FUNC::TRANSMITTER_TUNING2:
693 : // do nothing, used in tuning.cpp for transmitter based tuning
694 25 : break;
695 : #endif // AP_RC_TRANSMITTER_TUNING_ENABLED
696 :
697 273 : default:
698 273 : return RC_Channel::do_aux_function(trigger);
699 : }
700 1384 : return true;
701 : }
702 :
703 : // change air-mode status
704 15 : void RC_Channel_Copter::do_aux_function_change_air_mode(const AuxSwitchPos ch_flag)
705 : {
706 15 : switch (ch_flag) {
707 4 : case AuxSwitchPos::HIGH:
708 4 : copter.air_mode = AirMode::AIRMODE_ENABLED;
709 4 : break;
710 1 : case AuxSwitchPos::MIDDLE:
711 1 : break;
712 10 : case AuxSwitchPos::LOW:
713 10 : copter.air_mode = AirMode::AIRMODE_DISABLED;
714 10 : break;
715 : }
716 15 : }
717 :
718 : // change force flying status
719 0 : void RC_Channel_Copter::do_aux_function_change_force_flying(const AuxSwitchPos ch_flag)
720 : {
721 0 : switch (ch_flag) {
722 0 : case AuxSwitchPos::HIGH:
723 0 : copter.force_flying = true;
724 0 : break;
725 0 : case AuxSwitchPos::MIDDLE:
726 0 : break;
727 0 : case AuxSwitchPos::LOW:
728 0 : copter.force_flying = false;
729 0 : break;
730 : }
731 0 : }
732 :
733 : // note that this is a method on the RC_Channels object, not the
734 : // individual channel
735 : // save_trim - adds roll and pitch trims from the radio to ahrs
736 2 : void RC_Channels_Copter::save_trim()
737 : {
738 2 : float roll_trim_rad = 0.0;
739 2 : float pitch_trim_rad = 0.0;
740 :
741 : #if AP_COPTER_AHRS_AUTO_TRIM_ENABLED
742 2 : if (auto_trim.running) {
743 2 : auto_trim.running = false;
744 : } else {
745 : #endif
746 :
747 : // get roll and pitch trim adjustment
748 0 : copter.flightmode->get_pilot_desired_lean_angles_rad(roll_trim_rad, pitch_trim_rad, copter.attitude_control->lean_angle_max_rad(), copter.attitude_control->get_althold_lean_angle_max_rad());
749 :
750 : #if AP_COPTER_AHRS_AUTO_TRIM_ENABLED
751 : }
752 : #endif
753 :
754 : // save roll and pitch trim
755 2 : AP::ahrs().add_trim(roll_trim_rad, pitch_trim_rad);
756 2 : LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM);
757 2 : gcs().send_text(MAV_SEVERITY_INFO, "Trim saved");
758 2 : }
759 :
760 : #if AP_COPTER_AHRS_AUTO_TRIM_ENABLED
761 : // start/stop ahrs auto trim
762 12 : void RC_Channels_Copter::do_aux_function_ahrs_auto_trim(const RC_Channel::AuxSwitchPos ch_flag)
763 : {
764 12 : switch (ch_flag) {
765 4 : case RC_Channel::AuxSwitchPos::HIGH:
766 4 : if (!copter.flightmode->allows_auto_trim()) {
767 0 : gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim not allowed in this mode");
768 0 : break;
769 : }
770 4 : gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim running");
771 : // flash the leds
772 4 : AP_Notify::flags.save_trim = true;
773 4 : auto_trim.running = true;
774 4 : break;
775 3 : case RC_Channel::AuxSwitchPos::MIDDLE:
776 3 : break;
777 5 : case RC_Channel::AuxSwitchPos::LOW:
778 5 : if (auto_trim.running) {
779 2 : AP_Notify::flags.save_trim = false;
780 2 : save_trim();
781 : }
782 5 : break;
783 : }
784 12 : }
785 :
786 : // auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
787 : // meant to be called continuously while the pilot attempts to keep the copter level
788 2 : void RC_Channels_Copter::auto_trim_cancel()
789 : {
790 2 : auto_trim.running = false;
791 2 : AP_Notify::flags.save_trim = false;
792 2 : gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim cancelled");
793 : // restore original trims
794 2 : }
795 :
796 759512 : void RC_Channels_Copter::auto_trim_run()
797 : {
798 759512 : if (!auto_trim.running) {
799 759141 : return;
800 : }
801 :
802 : // only trim in certain modes:
803 371 : if (!copter.flightmode->allows_auto_trim()) {
804 2 : auto_trim_cancel();
805 2 : return;
806 : }
807 :
808 : // must be started and stopped mid-air:
809 369 : if (copter.ap.land_complete_maybe) {
810 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"Must be flying to use AUTOTRIM");
811 0 : auto_trim_cancel();
812 0 : return;
813 : }
814 : // calculate roll trim adjustment, divisor set subjectively to give same "feel" as previous RC input method
815 369 : float roll_trim_adjustment_rad = copter.attitude_control->get_att_target_euler_rad().x / 20.0f;
816 :
817 : // calculate pitch trim adjustment, divisor set subjectively to give same "feel" as previous RC input method
818 369 : float pitch_trim_adjustment_rad = copter.attitude_control->get_att_target_euler_rad().y / 20.0f;
819 :
820 : // add trim to ahrs object, but do not save to permanent storage:
821 369 : AP::ahrs().add_trim(roll_trim_adjustment_rad, pitch_trim_adjustment_rad, false);
822 : }
823 :
824 : #endif // AP_COPTER_AHRS_AUTO_TRIM_ENABLED
|
