CoCalc -- RC_Channel

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/ArduCopter/RC_Channel_Copter.cpp
Views: ¹⁷⁹⁸
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
int8_t RC_Channels_Copter::flight_mode_channel_number() const
13
{
14
    return copter.g.flight_mode_chan.get();
15
}
16

17
void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos)
18
{
19
    if (new_pos < 0 || (uint8_t)new_pos > copter.num_flight_modes) {
20
        // should not have been called
21
        return;
22
    }
23

24
    if (!copter.set_mode((Mode::Number)copter.flight_modes[new_pos].get(), ModeReason::RC_COMMAND)) {
25
        return;
26
    }
27

28
    if (!rc().find_channel_for_option(AUX_FUNC::SIMPLE_MODE) &&
29
        !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
        if (BIT_IS_SET(copter.g.super_simple, new_pos)) {
33
            copter.set_simple_mode(Copter::SimpleMode::SUPERSIMPLE);
34
        } else {
35
            copter.set_simple_mode(BIT_IS_SET(copter.g.simple_modes, new_pos) ? Copter::SimpleMode::SIMPLE : Copter::SimpleMode::NONE);
36
        }
37
    }
38
}
39

40
bool RC_Channels_Copter::in_rc_failsafe() const
41
{
42
    return copter.failsafe.radio;
43
}
44

45
bool RC_Channels_Copter::has_valid_input() const
46
{
47
    if (in_rc_failsafe()) {
48
        return false;
49
    }
50
    if (copter.failsafe.radio_counter != 0) {
51
        return false;
52
    }
53
    return true;
54
}
55

56
// returns true if throttle arming checks should be run
57
bool RC_Channels_Copter::arming_check_throttle() const {
58
    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
        return false;
62
    }
63
    return RC_Channels::arming_check_throttle();
64
}
65

66
RC_Channel * RC_Channels_Copter::get_arming_channel(void) const
67
{
68
    return copter.channel_yaw;
69
}
70

71
// init_aux_switch_function - initialize aux functions
72
void RC_Channel_Copter::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag)
73
{
74
    // init channel options
75
    switch(ch_option) {
76
    // the following functions do not need to be initialised:
77
    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
    case AUX_FUNC::SAVE_WP:
98
    case AUX_FUNC::SMART_RTL:
99
    case AUX_FUNC::STABILIZE:
100
    case AUX_FUNC::THROW:
101
    case AUX_FUNC::USER_FUNC1:
102
    case AUX_FUNC::USER_FUNC2:
103
    case AUX_FUNC::USER_FUNC3:
104
#if AP_WINCH_ENABLED
105
    case AUX_FUNC::WINCH_CONTROL:
106
#endif
107
    case AUX_FUNC::ZIGZAG:
108
    case AUX_FUNC::ZIGZAG_Auto:
109
    case AUX_FUNC::ZIGZAG_SaveWP:
110
    case AUX_FUNC::ACRO:
111
    case AUX_FUNC::AUTO_RTL:
112
    case AUX_FUNC::TURTLE:
113
    case AUX_FUNC::SIMPLE_HEADING_RESET:
114
    case AUX_FUNC::ARMDISARM_AIRMODE:
115
    case AUX_FUNC::TURBINE_START:
116
    case AUX_FUNC::FLIGHTMODE_PAUSE:
117
        break;
118
    case AUX_FUNC::ACRO_TRAINER:
119
    case AUX_FUNC::ATTCON_ACCEL_LIM:
120
    case AUX_FUNC::ATTCON_FEEDFWD:
121
    case AUX_FUNC::INVERTED:
122
    case AUX_FUNC::MOTOR_INTERLOCK:
123
#if HAL_PARACHUTE_ENABLED
124
    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
125
    case AUX_FUNC::PARACHUTE_ENABLE:
126
#endif
127
    case AUX_FUNC::PRECISION_LOITER:
128
#if AP_RANGEFINDER_ENABLED
129
    case AUX_FUNC::RANGEFINDER:
130
#endif
131
    case AUX_FUNC::SIMPLE_MODE:
132
    case AUX_FUNC::STANDBY:
133
    case AUX_FUNC::SUPERSIMPLE_MODE:
134
    case AUX_FUNC::SURFACE_TRACKING:
135
#if AP_WINCH_ENABLED
136
    case AUX_FUNC::WINCH_ENABLE:
137
#endif
138
    case AUX_FUNC::AIRMODE:
139
    case AUX_FUNC::FORCEFLYING:
140
    case AUX_FUNC::CUSTOM_CONTROLLER:
141
    case AUX_FUNC::WEATHER_VANE_ENABLE:
142
    case AUX_FUNC::TRANSMITTER_TUNING:
143
        run_aux_function(ch_option, ch_flag, AuxFuncTrigger::Source::INIT, ch_in);
144
        break;
145
    default:
146
        RC_Channel::init_aux_function(ch_option, ch_flag);
147
        break;
148
    }
149
}
150

151
// do_aux_function_change_mode - change mode based on an aux switch
152
// being moved
153
void RC_Channel_Copter::do_aux_function_change_mode(const Mode::Number mode,
154
                                                    const AuxSwitchPos ch_flag)
155
{
156
    switch(ch_flag) {
157
    case AuxSwitchPos::HIGH: {
158
        // engage mode (if not possible we remain in current flight mode)
159
        copter.set_mode(mode, ModeReason::AUX_FUNCTION);
160
        break;
161
    }
162
    default:
163
        // return to flight mode switch's flight mode if we are currently
164
        // in this mode
165
        if (copter.flightmode->mode_number() == mode) {
166
            rc().reset_mode_switch();
167
        }
168
    }
169
}
170

171
// do_aux_function - implement the function invoked by auxiliary switches
172
bool RC_Channel_Copter::do_aux_function(const AuxFuncTrigger &trigger)
173
{
174
    const AUX_FUNC &ch_option = trigger.func;
175
    const AuxSwitchPos &ch_flag = trigger.pos;
176

177
    switch(ch_option) {
178
        case AUX_FUNC::FLIP:
179
            // flip if switch is on, positive throttle and we're actually flying
180
            if (ch_flag == AuxSwitchPos::HIGH) {
181
                copter.set_mode(Mode::Number::FLIP, ModeReason::AUX_FUNCTION);
182
            }
183
            break;
184

185
        case AUX_FUNC::SIMPLE_MODE:
186
            // low = simple mode off, middle or high position turns simple mode on
187
            copter.set_simple_mode((ch_flag == AuxSwitchPos::LOW) ? Copter::SimpleMode::NONE : Copter::SimpleMode::SIMPLE);
188
            break;
189

190
        case AUX_FUNC::SUPERSIMPLE_MODE: {
191
            Copter::SimpleMode newmode = Copter::SimpleMode::NONE;
192
            switch (ch_flag) {
193
            case AuxSwitchPos::LOW:
194
                break;
195
            case AuxSwitchPos::MIDDLE:
196
                newmode = Copter::SimpleMode::SIMPLE;
197
                break;
198
            case AuxSwitchPos::HIGH:
199
                newmode = Copter::SimpleMode::SUPERSIMPLE;
200
                break;
201
            }
202
            copter.set_simple_mode(newmode);
203
            break;
204
        }
205

206
#if MODE_RTL_ENABLED
207
        case AUX_FUNC::RTL:
208
            do_aux_function_change_mode(Mode::Number::RTL, ch_flag);
209
            break;
210
#endif
211

212
        case AUX_FUNC::SAVE_TRIM:
213
            if ((ch_flag == AuxSwitchPos::HIGH) &&
214
                (copter.flightmode->allows_save_trim()) &&
215
                (copter.channel_throttle->get_control_in() == 0)) {
216
                copter.save_trim();
217
            }
218
            break;
219

220
#if MODE_AUTO_ENABLED
221
        case AUX_FUNC::SAVE_WP:
222
            // save waypoint when switch is brought high
223
            if (ch_flag == RC_Channel::AuxSwitchPos::HIGH) {
224

225
                // do not allow saving new waypoints while we're in auto or disarmed
226
                if (copter.flightmode == &copter.mode_auto || !copter.motors->armed()) {
227
                    break;
228
                }
229

230
                // do not allow saving the first waypoint with zero throttle
231
                if ((copter.mode_auto.mission.num_commands() == 0) && (copter.channel_throttle->get_control_in() == 0)) {
232
                    break;
233
                }
234

235
                // create new mission command
236
                AP_Mission::Mission_Command cmd  = {};
237

238
                // if the mission is empty save a takeoff command
239
                if (copter.mode_auto.mission.num_commands() == 0) {
240
                    // set our location ID to 16, MAV_CMD_NAV_WAYPOINT
241
                    cmd.id = MAV_CMD_NAV_TAKEOFF;
242
                    cmd.content.location.alt = MAX(copter.current_loc.alt,100);
243

244
                    // use the current altitude for the target alt for takeoff.
245
                    // only altitude will matter to the AP mission script for takeoff.
246
                    if (copter.mode_auto.mission.add_cmd(cmd)) {
247
                        // log event
248
                        LOGGER_WRITE_EVENT(LogEvent::SAVEWP_ADD_WP);
249
                    }
250
                }
251

252
                // set new waypoint to current location
253
                cmd.content.location = copter.current_loc;
254

255
                // if throttle is above zero, create waypoint command
256
                if (copter.channel_throttle->get_control_in() > 0) {
257
                    cmd.id = MAV_CMD_NAV_WAYPOINT;
258
                } else {
259
                    // with zero throttle, create LAND command
260
                    cmd.id = MAV_CMD_NAV_LAND;
261
                }
262

263
                // save command
264
                if (copter.mode_auto.mission.add_cmd(cmd)) {
265
                    // log event
266
                    LOGGER_WRITE_EVENT(LogEvent::SAVEWP_ADD_WP);
267
                }
268
            }
269
            break;
270

271
        case AUX_FUNC::AUTO:
272
            do_aux_function_change_mode(Mode::Number::AUTO, ch_flag);
273
            break;
274
#endif
275

276
#if AP_RANGEFINDER_ENABLED
277
        case AUX_FUNC::RANGEFINDER:
278
            // enable or disable the rangefinder
279
            if ((ch_flag == AuxSwitchPos::HIGH) &&
280
                copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
281
                copter.rangefinder_state.enabled = true;
282
            } else {
283
                copter.rangefinder_state.enabled = false;
284
            }
285
            break;
286
#endif // AP_RANGEFINDER_ENABLED
287

288
#if MODE_ACRO_ENABLED
289
        case AUX_FUNC::ACRO_TRAINER:
290
            switch(ch_flag) {
291
                case AuxSwitchPos::LOW:
292
                    copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::OFF);
293
                    LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_OFF);
294
                    break;
295
                case AuxSwitchPos::MIDDLE:
296
                    copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::LEVELING);
297
                    LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_LEVELING);
298
                    break;
299
                case AuxSwitchPos::HIGH:
300
                    copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::LIMITED);
301
                    LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_LIMITED);
302
                    break;
303
            }
304
            break;
305
#endif
306

307
#if AUTOTUNE_ENABLED
308
        case AUX_FUNC::AUTOTUNE_MODE:
309
            do_aux_function_change_mode(Mode::Number::AUTOTUNE, ch_flag);
310
            break;
311
        case AUX_FUNC::AUTOTUNE_TEST_GAINS:
312
            copter.mode_autotune.autotune.do_aux_function(ch_flag);
313
            break;
314
#endif
315

316
        case AUX_FUNC::LAND:
317
            do_aux_function_change_mode(Mode::Number::LAND, ch_flag);
318
            break;
319

320
        case AUX_FUNC::GUIDED:
321
            do_aux_function_change_mode(Mode::Number::GUIDED, ch_flag);
322
            break;
323

324
        case AUX_FUNC::LOITER:
325
            do_aux_function_change_mode(Mode::Number::LOITER, ch_flag);
326
            break;
327

328
        case AUX_FUNC::FOLLOW:
329
            do_aux_function_change_mode(Mode::Number::FOLLOW, ch_flag);
330
            break;
331

332
#if HAL_PARACHUTE_ENABLED
333
        case AUX_FUNC::PARACHUTE_ENABLE:
334
            // Parachute enable/disable
335
            copter.parachute.enabled(ch_flag == AuxSwitchPos::HIGH);
336
            break;
337

338
        case AUX_FUNC::PARACHUTE_RELEASE:
339
            if (ch_flag == AuxSwitchPos::HIGH) {
340
                copter.parachute_manual_release();
341
            }
342
            break;
343

344
        case AUX_FUNC::PARACHUTE_3POS:
345
            // Parachute disable, enable, release with 3 position switch
346
            switch (ch_flag) {
347
                case AuxSwitchPos::LOW:
348
                    copter.parachute.enabled(false);
349
                    break;
350
                case AuxSwitchPos::MIDDLE:
351
                    copter.parachute.enabled(true);
352
                    break;
353
                case AuxSwitchPos::HIGH:
354
                    copter.parachute.enabled(true);
355
                    copter.parachute_manual_release();
356
                    break;
357
            }
358
            break;
359
#endif  // HAL_PARACHUTE_ENABLED
360

361
        case AUX_FUNC::ATTCON_FEEDFWD:
362
            // enable or disable feed forward
363
            copter.attitude_control->bf_feedforward(ch_flag == AuxSwitchPos::HIGH);
364
            break;
365

366
        case AUX_FUNC::ATTCON_ACCEL_LIM:
367
            // enable or disable accel limiting by restoring defaults
368
            copter.attitude_control->accel_limiting(ch_flag == AuxSwitchPos::HIGH);
369
            break;
370

371
        case AUX_FUNC::MOTOR_INTERLOCK:
372
#if FRAME_CONFIG == HELI_FRAME
373
            // The interlock logic for ROTOR_CONTROL_MODE_PASSTHROUGH is handled 
374
            // in heli_update_rotor_speed_targets.  Otherwise turn on when above low.
375
            if (copter.motors->get_rsc_mode() != ROTOR_CONTROL_MODE_PASSTHROUGH) {
376
                copter.ap.motor_interlock_switch = (ch_flag == AuxSwitchPos::HIGH || ch_flag == AuxSwitchPos::MIDDLE);
377
            }
378
#else
379
            copter.ap.motor_interlock_switch = (ch_flag == AuxSwitchPos::HIGH || ch_flag == AuxSwitchPos::MIDDLE);
380
#endif
381
            break;
382

383
#if FRAME_CONFIG == HELI_FRAME
384
        case AUX_FUNC::TURBINE_START:
385
           switch (ch_flag) {
386
                case AuxSwitchPos::HIGH:
387
                    copter.motors->set_turb_start(true);
388
                    break;
389
                case AuxSwitchPos::MIDDLE:
390
                    // nothing
391
                    break;
392
                case AuxSwitchPos::LOW:
393
                    copter.motors->set_turb_start(false);
394
                    break;
395
           }
396
           break;
397
#endif
398

399
#if MODE_BRAKE_ENABLED
400
        case AUX_FUNC::BRAKE:
401
            do_aux_function_change_mode(Mode::Number::BRAKE, ch_flag);
402
            break;
403
#endif
404

405
#if MODE_THROW_ENABLED
406
        case AUX_FUNC::THROW:
407
            do_aux_function_change_mode(Mode::Number::THROW, ch_flag);
408
            break;
409
#endif
410

411
#if AC_PRECLAND_ENABLED && MODE_LOITER_ENABLED
412
        case AUX_FUNC::PRECISION_LOITER:
413
            switch (ch_flag) {
414
                case AuxSwitchPos::HIGH:
415
                    copter.mode_loiter.set_precision_loiter_enabled(true);
416
                    break;
417
                case AuxSwitchPos::MIDDLE:
418
                    // nothing
419
                    break;
420
                case AuxSwitchPos::LOW:
421
                    copter.mode_loiter.set_precision_loiter_enabled(false);
422
                    break;
423
            }
424
            break;
425
#endif
426

427
#if MODE_SMARTRTL_ENABLED
428
        case AUX_FUNC::SMART_RTL:
429
            do_aux_function_change_mode(Mode::Number::SMART_RTL, ch_flag);
430
            break;
431
#endif
432

433
#if FRAME_CONFIG == HELI_FRAME
434
        case AUX_FUNC::INVERTED:
435
            switch (ch_flag) {
436
            case AuxSwitchPos::HIGH:
437
                if (copter.flightmode->allows_inverted()) {
438
                    copter.attitude_control->set_inverted_flight(true);
439
                } else {
440
                    gcs().send_text(MAV_SEVERITY_WARNING, "Inverted flight not available in %s mode", copter.flightmode->name());
441
                }
442
                break;
443
            case AuxSwitchPos::MIDDLE:
444
                // nothing
445
                break;
446
            case AuxSwitchPos::LOW:
447
                copter.attitude_control->set_inverted_flight(false);
448
                break;
449
            }
450
            break;
451
#endif
452

453
#if AP_WINCH_ENABLED
454
        case AUX_FUNC::WINCH_ENABLE:
455
            switch (ch_flag) {
456
                case AuxSwitchPos::HIGH:
457
                    // high switch position stops winch using rate control
458
                    copter.g2.winch.set_desired_rate(0.0f);
459
                    break;
460
                case AuxSwitchPos::MIDDLE:
461
                case AuxSwitchPos::LOW:
462
                    // all other position relax winch
463
                    copter.g2.winch.relax();
464
                    break;
465
                }
466
            break;
467

468
        case AUX_FUNC::WINCH_CONTROL:
469
            // do nothing, used to control the rate of the winch and is processed within AP_Winch
470
            break;
471
#endif  // AP_WINCH_ENABLED
472

473
#ifdef USERHOOK_AUXSWITCH
474
        case AUX_FUNC::USER_FUNC1:
475
            copter.userhook_auxSwitch1(ch_flag);
476
            break;
477

478
        case AUX_FUNC::USER_FUNC2:
479
            copter.userhook_auxSwitch2(ch_flag);
480
            break;
481

482
        case AUX_FUNC::USER_FUNC3:
483
            copter.userhook_auxSwitch3(ch_flag);
484
            break;
485
#endif
486

487
#if MODE_ZIGZAG_ENABLED
488
        case AUX_FUNC::ZIGZAG:
489
            do_aux_function_change_mode(Mode::Number::ZIGZAG, ch_flag);
490
            break;
491

492
        case AUX_FUNC::ZIGZAG_SaveWP:
493
            if (copter.flightmode == &copter.mode_zigzag) {
494
                // initialize zigzag auto
495
                copter.mode_zigzag.init_auto();
496
                switch (ch_flag) {
497
                    case AuxSwitchPos::LOW:
498
                        copter.mode_zigzag.save_or_move_to_destination(ModeZigZag::Destination::A);
499
                        break;
500
                    case AuxSwitchPos::MIDDLE:
501
                        copter.mode_zigzag.return_to_manual_control(false);
502
                        break;
503
                    case AuxSwitchPos::HIGH:
504
                        copter.mode_zigzag.save_or_move_to_destination(ModeZigZag::Destination::B);
505
                        break;
506
                }
507
            }
508
            break;
509
#endif
510

511
        case AUX_FUNC::STABILIZE:
512
            do_aux_function_change_mode(Mode::Number::STABILIZE, ch_flag);
513
            break;
514

515
#if MODE_POSHOLD_ENABLED
516
        case AUX_FUNC::POSHOLD:
517
            do_aux_function_change_mode(Mode::Number::POSHOLD, ch_flag);
518
            break;
519
#endif
520

521
        case AUX_FUNC::ALTHOLD:
522
            do_aux_function_change_mode(Mode::Number::ALT_HOLD, ch_flag);
523
            break;
524

525
#if MODE_ACRO_ENABLED
526
        case AUX_FUNC::ACRO:
527
            do_aux_function_change_mode(Mode::Number::ACRO, ch_flag);
528
            break;
529
#endif
530

531
#if MODE_FLOWHOLD_ENABLED
532
        case AUX_FUNC::FLOWHOLD:
533
            do_aux_function_change_mode(Mode::Number::FLOWHOLD, ch_flag);
534
            break;
535
#endif
536

537
#if MODE_CIRCLE_ENABLED
538
        case AUX_FUNC::CIRCLE:
539
            do_aux_function_change_mode(Mode::Number::CIRCLE, ch_flag);
540
            break;
541
#endif
542

543
#if MODE_DRIFT_ENABLED
544
        case AUX_FUNC::DRIFT:
545
            do_aux_function_change_mode(Mode::Number::DRIFT, ch_flag);
546
            break;
547
#endif
548

549
        case AUX_FUNC::STANDBY: {
550
            switch (ch_flag) {
551
                case AuxSwitchPos::HIGH:
552
                    copter.standby_active = true;
553
                    LOGGER_WRITE_EVENT(LogEvent::STANDBY_ENABLE);
554
                    gcs().send_text(MAV_SEVERITY_INFO, "Stand By Enabled");
555
                    break;
556
                default:
557
                    copter.standby_active = false;
558
                    LOGGER_WRITE_EVENT(LogEvent::STANDBY_DISABLE);
559
                    gcs().send_text(MAV_SEVERITY_INFO, "Stand By Disabled");
560
                    break;
561
                }
562
            break;
563
        }
564

565
#if AP_RANGEFINDER_ENABLED
566
        case AUX_FUNC::SURFACE_TRACKING:
567
            switch (ch_flag) {
568
            case AuxSwitchPos::LOW:
569
                copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::GROUND);
570
                break;
571
            case AuxSwitchPos::MIDDLE:
572
                copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::NONE);
573
                break;
574
            case AuxSwitchPos::HIGH:
575
                copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::CEILING);
576
                break;
577
            }
578
            break;
579
#endif
580

581
        case AUX_FUNC::FLIGHTMODE_PAUSE:
582
            switch (ch_flag) {
583
                case AuxSwitchPos::HIGH:
584
                    if (!copter.flightmode->pause()) {
585
                        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Flight Mode Pause failed");
586
                    }
587
                    break;
588
                case AuxSwitchPos::MIDDLE:
589
                    break;
590
                case AuxSwitchPos::LOW:
591
                    copter.flightmode->resume();
592
                    break;
593
            }
594
            break;
595

596
#if MODE_ZIGZAG_ENABLED
597
        case AUX_FUNC::ZIGZAG_Auto:
598
            if (copter.flightmode == &copter.mode_zigzag) {
599
                switch (ch_flag) {
600
                case AuxSwitchPos::HIGH:
601
                    copter.mode_zigzag.run_auto();
602
                    break;
603
                default:
604
                    copter.mode_zigzag.suspend_auto();
605
                    break;
606
                }
607
            }
608
            break;
609
#endif
610

611
        case AUX_FUNC::AIRMODE:
612
            do_aux_function_change_air_mode(ch_flag);
613
#if MODE_ACRO_ENABLED && FRAME_CONFIG != HELI_FRAME
614
            copter.mode_acro.air_mode_aux_changed();
615
#endif
616
            break;
617

618
        case AUX_FUNC::FORCEFLYING:
619
            do_aux_function_change_force_flying(ch_flag);
620
            break;
621

622
#if MODE_AUTO_ENABLED
623
        case AUX_FUNC::AUTO_RTL:
624
            do_aux_function_change_mode(Mode::Number::AUTO_RTL, ch_flag);
625
            break;
626
#endif
627

628
#if MODE_TURTLE_ENABLED
629
        case AUX_FUNC::TURTLE:
630
            do_aux_function_change_mode(Mode::Number::TURTLE, ch_flag);
631
            break;
632
#endif
633

634
        case AUX_FUNC::SIMPLE_HEADING_RESET:
635
            if (ch_flag == AuxSwitchPos::HIGH) {
636
                copter.init_simple_bearing();
637
                gcs().send_text(MAV_SEVERITY_INFO, "Simple heading reset");
638
            }
639
            break;
640

641
        case AUX_FUNC::ARMDISARM_AIRMODE:
642
            RC_Channel::do_aux_function_armdisarm(ch_flag);
643
            if (copter.arming.is_armed()) {
644
                copter.ap.armed_with_airmode_switch = true;
645
            }
646
            break;
647

648
#if AC_CUSTOMCONTROL_MULTI_ENABLED
649
        case AUX_FUNC::CUSTOM_CONTROLLER:
650
            copter.custom_control.set_custom_controller(ch_flag == AuxSwitchPos::HIGH);
651
            break;
652
#endif
653

654
#if WEATHERVANE_ENABLED
655
    case AUX_FUNC::WEATHER_VANE_ENABLE: {
656
        switch (ch_flag) {
657
            case AuxSwitchPos::HIGH:
658
                copter.g2.weathervane.allow_weathervaning(true);
659
                break;
660
            case AuxSwitchPos::MIDDLE:
661
                break;
662
            case AuxSwitchPos::LOW:
663
                copter.g2.weathervane.allow_weathervaning(false);
664
                break;
665
        }
666
        break;
667
    }
668
#endif
669
    case AUX_FUNC::TRANSMITTER_TUNING:
670
        // do nothing, used in tuning.cpp for transmitter based tuning
671
        break;
672

673
    default:
674
        return RC_Channel::do_aux_function(trigger);
675
    }
676
    return true;
677
}
678

679
// change air-mode status
680
void RC_Channel_Copter::do_aux_function_change_air_mode(const AuxSwitchPos ch_flag)
681
{
682
    switch (ch_flag) {
683
    case AuxSwitchPos::HIGH:
684
        copter.air_mode = AirMode::AIRMODE_ENABLED;
685
        break;
686
    case AuxSwitchPos::MIDDLE:
687
        break;
688
    case AuxSwitchPos::LOW:
689
        copter.air_mode = AirMode::AIRMODE_DISABLED;
690
        break;
691
    }
692
}
693

694
// change force flying status
695
void RC_Channel_Copter::do_aux_function_change_force_flying(const AuxSwitchPos ch_flag)
696
{
697
    switch (ch_flag) {
698
    case AuxSwitchPos::HIGH:
699
        copter.force_flying = true;
700
        break;
701
    case AuxSwitchPos::MIDDLE:
702
        break;
703
    case AuxSwitchPos::LOW:
704
        copter.force_flying = false;
705
        break;
706
    }
707
}
708

709
// save_trim - adds roll and pitch trims from the radio to ahrs
710
void Copter::save_trim()
711
{
712
    // save roll and pitch trim
713
    float roll_trim = ToRad((float)channel_roll->get_control_in()*0.01f);
714
    float pitch_trim = ToRad((float)channel_pitch->get_control_in()*0.01f);
715
    ahrs.add_trim(roll_trim, pitch_trim);
716
    LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM);
717
    gcs().send_text(MAV_SEVERITY_INFO, "Trim saved");
718
}
719

720
// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
721
// meant to be called continuously while the pilot attempts to keep the copter level
722
void Copter::auto_trim_cancel()
723
{
724
    auto_trim_counter = 0;
725
    AP_Notify::flags.save_trim = false;
726
    gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim cancelled");
727
}
728

729
void Copter::auto_trim()
730
{
731
    if (auto_trim_counter > 0) {
732
        if (copter.flightmode != &copter.mode_stabilize ||
733
            !copter.motors->armed()) {
734
            auto_trim_cancel();
735
            return;
736
        }
737

738
        // flash the leds
739
        AP_Notify::flags.save_trim = true;
740

741
        if (!auto_trim_started) {
742
            if (ap.land_complete) {
743
                // haven't taken off yet
744
                return;
745
            }
746
            auto_trim_started = true;
747
        }
748

749
        if (ap.land_complete) {
750
            // landed again.
751
            auto_trim_cancel();
752
            return;
753
        }
754

755
        auto_trim_counter--;
756

757
        // calculate roll trim adjustment
758
        float roll_trim_adjustment = ToRad((float)channel_roll->get_control_in() / 4000.0f);
759

760
        // calculate pitch trim adjustment
761
        float pitch_trim_adjustment = ToRad((float)channel_pitch->get_control_in() / 4000.0f);
762

763
        // add trim to ahrs object
764
        // save to eeprom on last iteration
765
        ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
766

767
        // on last iteration restore leds and accel gains to normal
768
        if (auto_trim_counter == 0) {
769
            AP_Notify::flags.save_trim = false;
770
            gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim: Trims saved");
771
        }
772
    }
773
}
774

775
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
