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/ArduPlane/RC_Channel_Plane.cpp
Views: ¹⁷⁹⁸
1
#include "Plane.h"
2

3
#include "RC_Channel_Plane.h"
4
#include "qautotune.h"
5

6
// defining these two macros and including the RC_Channels_VarInfo
7
// header defines the parameter information common to all vehicle
8
// types
9
#define RC_CHANNELS_SUBCLASS RC_Channels_Plane
10
#define RC_CHANNEL_SUBCLASS RC_Channel_Plane
11

12
#include <RC_Channel/RC_Channels_VarInfo.h>
13

14
// note that this callback is not presently used on Plane:
15
int8_t RC_Channels_Plane::flight_mode_channel_number() const
16
{
17
    return plane.g.flight_mode_channel.get();
18
}
19

20
bool RC_Channels_Plane::in_rc_failsafe() const
21
{
22
    return (plane.rc_failsafe_active() || plane.failsafe.rc_failsafe);
23
}
24

25
bool RC_Channels_Plane::has_valid_input() const
26
{
27
    if (in_rc_failsafe()) {
28
        return false;
29
    }
30
    if (plane.failsafe.throttle_counter != 0) {
31
        return false;
32
    }
33
    return true;
34
}
35

36
RC_Channel * RC_Channels_Plane::get_arming_channel(void) const
37
{
38
    return plane.channel_rudder;
39
}
40

41
void RC_Channel_Plane::do_aux_function_change_mode(const Mode::Number number,
42
                                                   const AuxSwitchPos ch_flag)
43
{
44
    switch(ch_flag) {
45
    case AuxSwitchPos::HIGH: {
46
        // engage mode (if not possible we remain in current flight mode)
47
        plane.set_mode_by_number(number, ModeReason::AUX_FUNCTION);
48
        break;
49
    }
50
    default:
51
        // return to flight mode switch's flight mode if we are currently
52
        // in this mode
53
        if (plane.control_mode->mode_number() == number) {
54
            rc().reset_mode_switch();
55
        }
56
    }
57
}
58

59
#if HAL_QUADPLANE_ENABLED
60
void RC_Channel_Plane::do_aux_function_q_assist_state(AuxSwitchPos ch_flag)
61
{
62
    switch(ch_flag) {
63
        case AuxSwitchPos::HIGH:
64
            gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Force enabled");
65
            plane.quadplane.assist.set_state(VTOL_Assist::STATE::FORCE_ENABLED);
66
            break;
67

68
        case AuxSwitchPos::MIDDLE:
69
            gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Enabled");
70
            plane.quadplane.assist.set_state(VTOL_Assist::STATE::ASSIST_ENABLED);
71
            break;
72

73
        case AuxSwitchPos::LOW:
74
            gcs().send_text(MAV_SEVERITY_INFO, "QAssist: Disabled");
75
            plane.quadplane.assist.set_state(VTOL_Assist::STATE::ASSIST_DISABLED);
76
            break;
77
    }
78
}
79
#endif  // HAL_QUADPLANE_ENABLED
80

81
void RC_Channel_Plane::do_aux_function_crow_mode(AuxSwitchPos ch_flag)
82
{
83
        switch(ch_flag) {
84
        case AuxSwitchPos::HIGH:
85
            plane.crow_mode = Plane::CrowMode::CROW_DISABLED;
86
            gcs().send_text(MAV_SEVERITY_INFO, "Crow Flaps Disabled");
87
            break;
88
        case AuxSwitchPos::MIDDLE:
89
            gcs().send_text(MAV_SEVERITY_INFO, "Progressive Crow Flaps"); 
90
            plane.crow_mode = Plane::CrowMode::PROGRESSIVE;   
91
            break;
92
        case AuxSwitchPos::LOW:
93
            plane.crow_mode = Plane::CrowMode::NORMAL;
94
            gcs().send_text(MAV_SEVERITY_INFO, "Normal Crow Flaps");
95
            break;
96
        }    
97
}
98

99
#if HAL_SOARING_ENABLED
100
void RC_Channel_Plane::do_aux_function_soaring_3pos(AuxSwitchPos ch_flag)
101
{
102
    SoaringController::ActiveStatus desired_state = SoaringController::ActiveStatus::SOARING_DISABLED;
103

104
    switch (ch_flag) {
105
        case AuxSwitchPos::LOW:
106
            desired_state = SoaringController::ActiveStatus::SOARING_DISABLED;
107
            break;
108
        case AuxSwitchPos::MIDDLE:
109
            desired_state = SoaringController::ActiveStatus::MANUAL_MODE_CHANGE;
110
            break;
111
        case AuxSwitchPos::HIGH:
112
            desired_state = SoaringController::ActiveStatus::AUTO_MODE_CHANGE;
113
            break;
114
        }
115

116
    plane.g2.soaring_controller.set_pilot_desired_state(desired_state);
117
}
118
#endif
119

120
void RC_Channel_Plane::do_aux_function_flare(AuxSwitchPos ch_flag)
121
{
122
        switch(ch_flag) {
123
        case AuxSwitchPos::HIGH:
124
            plane.flare_mode = Plane::FlareMode::ENABLED_PITCH_TARGET;
125
            break;
126
        case AuxSwitchPos::MIDDLE:
127
            plane.flare_mode = Plane::FlareMode::ENABLED_NO_PITCH_TARGET;
128
            break;
129
        case AuxSwitchPos::LOW:
130
            plane.flare_mode = Plane::FlareMode::FLARE_DISABLED;
131
            break;
132
        }    
133
}
134

135

136
void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option,
137
                                         const RC_Channel::AuxSwitchPos ch_flag)
138
{
139
    switch(ch_option) {
140
    // the following functions do not need to be initialised:
141
    case AUX_FUNC::AUTO:
142
    case AUX_FUNC::CIRCLE:
143
    case AUX_FUNC::ACRO:
144
    case AUX_FUNC::TRAINING:
145
    case AUX_FUNC::FLAP:
146
    case AUX_FUNC::GUIDED:
147
    case AUX_FUNC::INVERTED:
148
    case AUX_FUNC::LOITER:
149
    case AUX_FUNC::MANUAL:
150
    case AUX_FUNC::RTL:
151
    case AUX_FUNC::TAKEOFF:
152
    case AUX_FUNC::FBWA:
153
    case AUX_FUNC::AIRBRAKE:
154
#if HAL_QUADPLANE_ENABLED
155
    case AUX_FUNC::QRTL:
156
    case AUX_FUNC::QSTABILIZE:
157
#endif
158
    case AUX_FUNC::FBWA_TAILDRAGGER:
159
    case AUX_FUNC::FWD_THR:
160
    case AUX_FUNC::LANDING_FLARE:
161
#if HAL_PARACHUTE_ENABLED
162
    case AUX_FUNC::PARACHUTE_RELEASE:
163
#endif
164
    case AUX_FUNC::MODE_SWITCH_RESET:
165
    case AUX_FUNC::CRUISE:
166
#if HAL_QUADPLANE_ENABLED
167
    case AUX_FUNC::ARMDISARM_AIRMODE:
168
#endif
169
    case AUX_FUNC::PLANE_AUTO_LANDING_ABORT:
170
    case AUX_FUNC::TRIM_TO_CURRENT_SERVO_RC:
171
    case AUX_FUNC::EMERGENCY_LANDING_EN:
172
    case AUX_FUNC::FW_AUTOTUNE:
173
    case AUX_FUNC::VFWD_THR_OVERRIDE:
174
    case AUX_FUNC::PRECISION_LOITER:
175
#if QAUTOTUNE_ENABLED
176
    case AUX_FUNC::AUTOTUNE_TEST_GAINS:
177
#endif
178
#if AP_QUICKTUNE_ENABLED
179
    case AUX_FUNC::QUICKTUNE:
180
#endif
181
        break;
182

183
    case AUX_FUNC::SOARING:
184
#if HAL_QUADPLANE_ENABLED
185
    case AUX_FUNC::Q_ASSIST:
186
    case AUX_FUNC::AIRMODE:
187
    case AUX_FUNC::WEATHER_VANE_ENABLE:
188
#endif
189
#if AP_AIRSPEED_AUTOCAL_ENABLE
190
    case AUX_FUNC::ARSPD_CALIBRATE:
191
#endif
192
    case AUX_FUNC::TER_DISABLE:
193
    case AUX_FUNC::CROW_SELECT:
194
#if AP_ICENGINE_ENABLED
195
    case AUX_FUNC::ICE_START_STOP:
196
#endif
197
        run_aux_function(ch_option, ch_flag, AuxFuncTrigger::Source::INIT, ch_in);
198
        break;
199

200
    case AUX_FUNC::REVERSE_THROTTLE:
201
        plane.have_reverse_throttle_rc_option = true;
202
        // setup input throttle as a range. This is needed as init_aux_function is called
203
        // after set_control_channels()
204
        if (plane.channel_throttle) {
205
            plane.channel_throttle->set_range(100);
206
        }
207
        // note that we don't call do_aux_function() here as we don't
208
        // want to startup with reverse thrust
209
        break;
210

211
    default:
212
        // handle in parent class
213
        RC_Channel::init_aux_function(ch_option, ch_flag);
214
        break;
215
    }
216
}
217

218
// do_aux_function - implement the function invoked by auxiliary switches
219
bool RC_Channel_Plane::do_aux_function(const AuxFuncTrigger &trigger)
220
{
221
    const AUX_FUNC &ch_option = trigger.func;
222
    const AuxSwitchPos &ch_flag = trigger.pos;
223

224
    switch(ch_option) {
225
    case AUX_FUNC::INVERTED:
226
        plane.inverted_flight = (ch_flag == AuxSwitchPos::HIGH);
227
        break;
228

229
    case AUX_FUNC::REVERSE_THROTTLE:
230
        plane.reversed_throttle = (ch_flag == AuxSwitchPos::HIGH);
231
        gcs().send_text(MAV_SEVERITY_INFO, "RevThrottle: %s", plane.reversed_throttle?"ENABLE":"DISABLE");
232
        break;
233

234
    case AUX_FUNC::AUTO:
235
        do_aux_function_change_mode(Mode::Number::AUTO, ch_flag);
236
        break;
237

238
    case AUX_FUNC::CIRCLE:
239
        do_aux_function_change_mode(Mode::Number::CIRCLE, ch_flag);
240
        break;
241

242
    case AUX_FUNC::ACRO:
243
        do_aux_function_change_mode(Mode::Number::ACRO, ch_flag);
244
        break;
245

246
    case AUX_FUNC::TRAINING:
247
        do_aux_function_change_mode(Mode::Number::TRAINING, ch_flag);
248
        break;
249
        
250
    case AUX_FUNC::LOITER:
251
        do_aux_function_change_mode(Mode::Number::LOITER, ch_flag);
252
        break;        
253

254
    case AUX_FUNC::GUIDED:
255
        do_aux_function_change_mode(Mode::Number::GUIDED, ch_flag);
256
        break;
257

258
    case AUX_FUNC::MANUAL:
259
        do_aux_function_change_mode(Mode::Number::MANUAL, ch_flag);
260
        break;
261

262
    case AUX_FUNC::RTL:
263
        do_aux_function_change_mode(Mode::Number::RTL, ch_flag);
264
        break;
265

266
    case AUX_FUNC::TAKEOFF:
267
        do_aux_function_change_mode(Mode::Number::TAKEOFF, ch_flag);
268
        break;
269

270
    case AUX_FUNC::FBWA:
271
        do_aux_function_change_mode(Mode::Number::FLY_BY_WIRE_A, ch_flag);
272
        break;
273

274
#if HAL_QUADPLANE_ENABLED
275
    case AUX_FUNC::QRTL:
276
        do_aux_function_change_mode(Mode::Number::QRTL, ch_flag);
277
        break;
278

279
    case AUX_FUNC::QSTABILIZE:
280
        do_aux_function_change_mode(Mode::Number::QSTABILIZE, ch_flag);
281
        break;
282

283
    case AUX_FUNC::VFWD_THR_OVERRIDE: {
284
        const bool enable = (ch_flag == AuxSwitchPos::HIGH);
285
        if (enable != plane.quadplane.vfwd_enable_active) {
286
            plane.quadplane.vfwd_enable_active = enable;
287
            gcs().send_text(MAV_SEVERITY_INFO, "QFwdThr: %s", enable?"ON":"OFF");
288
        }
289
        break;
290
    }
291
#endif
292

293
#if HAL_SOARING_ENABLED
294
    case AUX_FUNC::SOARING:
295
        do_aux_function_soaring_3pos(ch_flag);
296
        break;
297
#endif
298

299
    case AUX_FUNC::FLAP:
300
    case AUX_FUNC::FBWA_TAILDRAGGER:
301
    case AUX_FUNC::AIRBRAKE:
302
        break; // input labels, nothing to do
303

304
#if HAL_QUADPLANE_ENABLED
305
    case AUX_FUNC::Q_ASSIST:
306
        do_aux_function_q_assist_state(ch_flag);
307
        break;
308
#endif
309

310
    case AUX_FUNC::FWD_THR:
311
        break; // VTOL forward throttle input label, nothing to do
312

313
    case AUX_FUNC::TER_DISABLE:
314
            switch (ch_flag) {
315
            case AuxSwitchPos::HIGH:
316
                plane.non_auto_terrain_disable = true;
317
                if (plane.control_mode->allows_terrain_disable()) {
318
                    plane.set_target_altitude_current();
319
                }
320
                break;
321
            case AuxSwitchPos::MIDDLE:
322
                break;
323
            case AuxSwitchPos::LOW:
324
                plane.non_auto_terrain_disable = false;
325
                if (plane.control_mode->allows_terrain_disable()) {
326
                    plane.set_target_altitude_current();
327
                }
328
                break;
329
            }
330
            gcs().send_text(MAV_SEVERITY_INFO, "NON AUTO TERRN: %s", plane.non_auto_terrain_disable?"OFF":"ON");
331
        break;
332

333
    case AUX_FUNC::CROW_SELECT:
334
        do_aux_function_crow_mode(ch_flag);
335
        break;
336

337
#if HAL_QUADPLANE_ENABLED
338
    case AUX_FUNC::AIRMODE:
339
        switch (ch_flag) {
340
        case AuxSwitchPos::HIGH:
341
            plane.quadplane.air_mode = AirMode::ON;
342
            plane.quadplane.throttle_wait = false;
343
            break;
344
        case AuxSwitchPos::MIDDLE:
345
            break;
346
        case AuxSwitchPos::LOW:
347
            plane.quadplane.air_mode = AirMode::OFF;
348
            break;
349
        }
350
        break;
351
#endif
352

353
#if AP_AIRSPEED_AUTOCAL_ENABLE
354
    case AUX_FUNC::ARSPD_CALIBRATE:
355
        switch (ch_flag) {
356
        case AuxSwitchPos::HIGH:
357
            plane.airspeed.set_calibration_enabled(true);
358
            break;
359
        case AuxSwitchPos::MIDDLE:
360
            break;
361
        case AuxSwitchPos::LOW:
362
            plane.airspeed.set_calibration_enabled(false);
363
            break;
364
        }
365
        break;
366
#endif
367

368
    case AUX_FUNC::LANDING_FLARE:
369
        do_aux_function_flare(ch_flag);
370
        break;
371

372
#if HAL_PARACHUTE_ENABLED
373
    case AUX_FUNC::PARACHUTE_RELEASE:
374
        if (ch_flag == AuxSwitchPos::HIGH) {
375
            plane.parachute_manual_release();
376
        }
377
        break;
378
#endif
379

380
    case AUX_FUNC::MODE_SWITCH_RESET:
381
        rc().reset_mode_switch();
382
        break;
383

384
    case AUX_FUNC::CRUISE:
385
        do_aux_function_change_mode(Mode::Number::CRUISE, ch_flag);
386
        break;
387

388
#if HAL_QUADPLANE_ENABLED
389
    case AUX_FUNC::ARMDISARM_AIRMODE:
390
        RC_Channel::do_aux_function_armdisarm(ch_flag);
391
        if (plane.arming.is_armed()) {
392
            plane.quadplane.air_mode = AirMode::ON;
393
            plane.quadplane.throttle_wait = false;
394
        }
395
        break;
396

397
    case AUX_FUNC::WEATHER_VANE_ENABLE: {
398
        if (plane.quadplane.weathervane != nullptr) {
399
            switch (ch_flag) {
400
                case AuxSwitchPos::HIGH:
401
                    plane.quadplane.weathervane->allow_weathervaning(true);
402
                    break;
403
                case AuxSwitchPos::MIDDLE:
404
                    // nothing
405
                    break;
406
                case AuxSwitchPos::LOW:
407
                    plane.quadplane.weathervane->allow_weathervaning(false);
408
                    break;
409
            }
410
        }
411
        break;
412
    }
413
#endif
414

415
    case AUX_FUNC::PLANE_AUTO_LANDING_ABORT:
416
        switch(ch_flag) {
417
        case AuxSwitchPos::HIGH:
418
            IGNORE_RETURN(plane.trigger_land_abort(0));
419
            break;
420
        case AuxSwitchPos::MIDDLE:
421
        case AuxSwitchPos::LOW:
422
            break;
423
        }
424
        break;
425

426
    case AUX_FUNC::TRIM_TO_CURRENT_SERVO_RC:
427
        if (ch_flag == AuxSwitchPos::HIGH) {
428
            plane.trim_radio();
429
        }
430
        break;
431

432
    case AUX_FUNC::EMERGENCY_LANDING_EN:
433
        switch (ch_flag) {
434
        case AuxSwitchPos::HIGH:
435
            plane.emergency_landing = true;
436
            break;
437
        case AuxSwitchPos::MIDDLE:
438
            break;
439
        case AuxSwitchPos::LOW:
440
            plane.emergency_landing = false;
441
            break;
442
        }
443
        break;
444

445
    case AUX_FUNC::FW_AUTOTUNE:
446
        if (ch_flag == AuxSwitchPos::HIGH && plane.control_mode->mode_allows_autotuning()) {
447
           plane.autotune_enable(true);
448
        } else if (ch_flag == AuxSwitchPos::HIGH) {
449
           GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Autotuning not allowed in this mode!");
450
        } else {
451
           plane.autotune_enable(false); 
452
        }
453
        break;
454

455
    case AUX_FUNC::PRECISION_LOITER:
456
        // handled by lua scripting, just ignore here
457
        break;
458

459
#if QAUTOTUNE_ENABLED
460
    case AUX_FUNC::AUTOTUNE_TEST_GAINS:
461
        plane.quadplane.qautotune.do_aux_function(ch_flag);
462
        break;
463
#endif
464

465
#if AP_QUICKTUNE_ENABLED
466
    case AUX_FUNC::QUICKTUNE:
467
        plane.quicktune.update_switch_pos(ch_flag);
468
        break;
469
#endif
470

471
#if AP_ICENGINE_ENABLED
472
    case AUX_FUNC::ICE_START_STOP:
473
        plane.g2.ice_control.do_aux_function(trigger);
474
        break;
475
#endif
476

477
    default:
478
        return RC_Channel::do_aux_function(trigger);
479
    }
480

481
    return true;
482
}
483

484
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.
