CoCalc -- joystick.cpp

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/ArduSub/joystick.cpp
Views: ¹⁷⁹⁸
1
#include "Sub.h"
2
#include "mode.h"
3

4
// Functions that will handle joystick/gamepad input
5
// ----------------------------------------------------------------------------
6

7
// Anonymous namespace to hold variables used only in this file
8
namespace {
9
float cam_tilt = 1500.0;
10
float cam_pan = 1500.0;
11
int16_t lights1 = 1100;
12
int16_t lights2 = 1100;
13
int16_t rollTrim = 0;
14
int16_t pitchTrim = 0;
15
int16_t zTrim = 0;
16
int16_t xTrim = 0;
17
int16_t yTrim = 0;
18
int16_t video_switch = 1100;
19
int16_t x_last, y_last, z_last;
20
uint32_t buttons_prev;
21

22
// Servo control output channels
23
// TODO: Allow selecting output channels
24
const uint8_t SERVO_CHAN_1 = 9; // Pixhawk Aux1
25
const uint8_t SERVO_CHAN_2 = 10; // Pixhawk Aux2
26
const uint8_t SERVO_CHAN_3 = 11; // Pixhawk Aux3
27

28
bool controls_reset_since_input_hold = true;
29
}
30

31
void Sub::init_joystick()
32
{
33
    default_js_buttons();
34

35
    lights1 = RC_Channels::rc_channel(8)->get_radio_min();
36
    lights2 = RC_Channels::rc_channel(9)->get_radio_min();
37

38
    set_mode(Mode::Number::MANUAL, ModeReason::RC_COMMAND); // Initialize flight mode
39

40
    if (g.numGainSettings < 1) {
41
        g.numGainSettings.set_and_save(1);
42
    }
43

44
    if (g.numGainSettings == 1 || (g.gain_default < g.maxGain + 0.01 && g.gain_default > g.minGain - 0.01)) {
45
        gain = constrain_float(g.gain_default, g.minGain, g.maxGain); // Use default gain parameter
46
    } else {
47
        // Use setting closest to average of minGain and maxGain
48
        gain = g.minGain + (g.numGainSettings/2 - 1) * (g.maxGain - g.minGain) / (g.numGainSettings - 1);
49
    }
50

51
    gain = constrain_float(gain, 0.1, 1.0);
52
}
53

54
void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions,
55
            int16_t s,
56
            int16_t t,
57
            int16_t aux1,
58
            int16_t aux2,
59
            int16_t aux3,
60
            int16_t aux4,
61
            int16_t aux5,
62
            int16_t aux6)
63
{
64

65
    float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain
66
    float throttleScale = 0.8*gain*g.throttle_gain; // Scale 0-1000 to 0-800 times gain
67
    int16_t rpyCenter = 1500;
68
    int16_t throttleBase = 1500-500*throttleScale;
69

70
    bool shift = false;
71

72
    // Neutralize camera tilt and pan speed setpoint
73
    cam_tilt = 1500;
74
    cam_pan = 1500;
75

76
    uint32_t all_buttons = buttons | (buttons2 << 16);
77
    // Detect if any shift button is pressed
78
    for (uint8_t i = 0 ; i < 32 ; i++) {
79
        if ((all_buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) {
80
            shift = true;
81
        }
82
    }
83

84
    // Act if button is pressed
85
    // Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick.
86
    for (uint8_t i = 0 ; i < 32 ; i++) {
87
        if ((all_buttons & (1 << i))) {
88
            handle_jsbutton_press(i,shift,(buttons_prev & (1 << i)));
89
            // buttonDebounce = tnow_ms;
90
        } else if (buttons_prev & (1 << i)) {
91
            handle_jsbutton_release(i, shift);
92
        }
93
    }
94

95
    buttons_prev = all_buttons;
96

97
    // attitude mode:
98
    if (roll_pitch_flag == 1) {
99
    // adjust roll/pitch trim with joystick input instead of forward/lateral
100
        pitchTrim = -x * rpyScale;
101
        rollTrim  =  y * rpyScale;
102
    }
103

104
    uint32_t tnow = AP_HAL::millis();
105

106
    int16_t zTot;
107
    int16_t yTot;
108
    int16_t xTot;
109

110
    if (!controls_reset_since_input_hold) {
111
        zTot = zTrim + 500; // 500 is neutral for throttle
112
        yTot = yTrim;
113
        xTot = xTrim;
114
        // if all 3 axes return to neutral, than we're ready to accept input again
115
        controls_reset_since_input_hold = (abs(z - 500) < 50) && (abs(y) < 50) && (abs(x) < 50);
116
    } else {
117
        zTot = z + zTrim;
118
        yTot = y + yTrim;
119
        xTot = x + xTrim;
120
    }
121

122
    RC_Channels::set_override(0, constrain_int16(s + pitchTrim + rpyCenter,1100,1900), tnow); // pitch
123
    RC_Channels::set_override(1, constrain_int16(t + rollTrim  + rpyCenter,1100,1900), tnow); // roll
124

125
    RC_Channels::set_override(2, constrain_int16((zTot)*throttleScale+throttleBase,1100,1900), tnow); // throttle
126
    RC_Channels::set_override(3, constrain_int16(r*rpyScale+rpyCenter,1100,1900), tnow);                 // yaw
127

128
    // maneuver mode:
129
    if (roll_pitch_flag == 0) {
130
        // adjust forward and lateral with joystick input instead of roll and pitch
131
        RC_Channels::set_override(4, constrain_int16((xTot)*rpyScale+rpyCenter,1100,1900), tnow); // forward for ROV
132
        RC_Channels::set_override(5, constrain_int16((yTot)*rpyScale+rpyCenter,1100,1900), tnow); // lateral for ROV
133
    } else {
134
        // neutralize forward and lateral input while we are adjusting roll and pitch
135
        RC_Channels::set_override(4, constrain_int16(xTrim*rpyScale+rpyCenter,1100,1900), tnow); // forward for ROV
136
        RC_Channels::set_override(5, constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900), tnow); // lateral for ROV
137
    }
138

139
    RC_Channels::set_override(6, cam_pan, tnow);       // camera pan
140
    RC_Channels::set_override(7, cam_tilt, tnow);      // camera tilt
141
    RC_Channels::set_override(8, lights1, tnow);       // lights 1
142
    RC_Channels::set_override(9, lights2, tnow);       // lights 2
143
    RC_Channels::set_override(10, video_switch, tnow); // video switch
144

145
    // Store old x, y, z values for use in input hold logic
146
    x_last = x;
147
    y_last = y;
148
    z_last = z;
149
}
150

151
void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
152
{
153
    // Act based on the function assigned to this button
154
    switch (get_button(_button)->function(shift)) {
155
    case JSButton::button_function_t::k_arm_toggle:
156
        if (motors.armed()) {
157
            arming.disarm(AP_Arming::Method::MAVLINK);
158
        } else {
159
            arming.arm(AP_Arming::Method::MAVLINK);
160
        }
161
        break;
162
    case JSButton::button_function_t::k_arm:
163
        arming.arm(AP_Arming::Method::MAVLINK);
164
        break;
165
    case JSButton::button_function_t::k_disarm:
166
        arming.disarm(AP_Arming::Method::MAVLINK);
167
        break;
168

169
    case JSButton::button_function_t::k_mode_manual:
170
        set_mode(Mode::Number::MANUAL, ModeReason::RC_COMMAND);
171
        break;
172
    case JSButton::button_function_t::k_mode_stabilize:
173
        set_mode(Mode::Number::STABILIZE, ModeReason::RC_COMMAND);
174
        break;
175
    case JSButton::button_function_t::k_mode_depth_hold:
176
        set_mode(Mode::Number::ALT_HOLD, ModeReason::RC_COMMAND);
177
        break;
178
    case JSButton::button_function_t::k_mode_auto:
179
        set_mode(Mode::Number::AUTO, ModeReason::RC_COMMAND);
180
        break;
181
    case JSButton::button_function_t::k_mode_guided:
182
        set_mode(Mode::Number::GUIDED, ModeReason::RC_COMMAND);
183
        break;
184
    case JSButton::button_function_t::k_mode_circle:
185
        set_mode(Mode::Number::CIRCLE, ModeReason::RC_COMMAND);
186
        break;
187
    case JSButton::button_function_t::k_mode_acro:
188
        set_mode(Mode::Number::ACRO, ModeReason::RC_COMMAND);
189
        break;
190
    case JSButton::button_function_t::k_mode_poshold:
191
        set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND);
192
        break;
193
#if AP_RANGEFINDER_ENABLED
194
    case JSButton::button_function_t::k_mode_surftrak:
195
        set_mode(Mode::Number::SURFTRAK, ModeReason::RC_COMMAND);
196
        break;
197
#endif
198

199
    case JSButton::button_function_t::k_mount_center:
200
#if HAL_MOUNT_ENABLED
201
        camera_mount.set_angle_target(0, 0, 0, false);
202
        // for some reason the call to set_angle_targets changes the mode to mavlink targeting!
203
        camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING);
204
#endif
205
        break;
206
    case JSButton::button_function_t::k_mount_tilt_up:
207
        cam_tilt = 1900;
208
        break;
209
    case JSButton::button_function_t::k_mount_tilt_down:
210
        cam_tilt = 1100;
211
        break;
212
    case JSButton::button_function_t::k_camera_trigger:
213
        break;
214
    case JSButton::button_function_t::k_camera_source_toggle:
215
        if (!held) {
216
            static bool video_toggle = false;
217
            video_toggle = !video_toggle;
218
            if (video_toggle) {
219
                video_switch = 1900;
220
                gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 2");
221
            } else {
222
                video_switch = 1100;
223
                gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 1");
224
            }
225
        }
226
        break;
227
    case JSButton::button_function_t::k_mount_pan_right:
228
        cam_pan = 1900;
229
        break;
230
    case JSButton::button_function_t::k_mount_pan_left:
231
        cam_pan = 1100;
232
        break;
233
    case JSButton::button_function_t::k_lights1_cycle:
234
        if (!held) {
235
            static bool increasing = true;
236
            RC_Channel* chan = RC_Channels::rc_channel(8);
237
            uint16_t min = chan->get_radio_min();
238
            uint16_t max = chan->get_radio_max();
239
            uint16_t step = (max - min) / g.lights_steps;
240
            if (increasing) {
241
                lights1 = constrain_float(lights1 + step, min, max);
242
            } else {
243
                lights1 = constrain_float(lights1 - step, min, max);
244
            }
245
            if (lights1 >= max || lights1 <= min) {
246
                increasing = !increasing;
247
            }
248
        }
249
        break;
250
    case JSButton::button_function_t::k_lights1_brighter:
251
        if (!held) {
252
            RC_Channel* chan = RC_Channels::rc_channel(8);
253
            uint16_t min = chan->get_radio_min();
254
            uint16_t max = chan->get_radio_max();
255
            uint16_t step = (max - min) / g.lights_steps;
256
            lights1 = constrain_float(lights1 + step, min, max);
257
        }
258
        break;
259
    case JSButton::button_function_t::k_lights1_dimmer:
260
        if (!held) {
261
            RC_Channel* chan = RC_Channels::rc_channel(8);
262
            uint16_t min = chan->get_radio_min();
263
            uint16_t max = chan->get_radio_max();
264
            uint16_t step = (max - min) / g.lights_steps;
265
            lights1 = constrain_float(lights1 - step, min, max);
266
        }
267
        break;
268
    case JSButton::button_function_t::k_lights2_cycle:
269
        if (!held) {
270
            static bool increasing = true;
271
            RC_Channel* chan = RC_Channels::rc_channel(9);
272
            uint16_t min = chan->get_radio_min();
273
            uint16_t max = chan->get_radio_max();
274
            uint16_t step = (max - min) / g.lights_steps;
275
            if (increasing) {
276
                lights2 = constrain_float(lights2 + step, min, max);
277
            } else {
278
                lights2 = constrain_float(lights2 - step, min, max);
279
            }
280
            if (lights2 >= max || lights2 <= min) {
281
                increasing = !increasing;
282
            }
283
        }
284
        break;
285
    case JSButton::button_function_t::k_lights2_brighter:
286
        if (!held) {
287
            RC_Channel* chan = RC_Channels::rc_channel(9);
288
            uint16_t min = chan->get_radio_min();
289
            uint16_t max = chan->get_radio_max();
290
            uint16_t step = (max - min) / g.lights_steps;
291
            lights2 = constrain_float(lights2 + step, min, max);
292
        }
293
        break;
294
    case JSButton::button_function_t::k_lights2_dimmer:
295
        if (!held) {
296
            RC_Channel* chan = RC_Channels::rc_channel(9);
297
            uint16_t min = chan->get_radio_min();
298
            uint16_t max = chan->get_radio_max();
299
            uint16_t step = (max - min) / g.lights_steps;
300
            lights2 = constrain_float(lights2 - step, min, max);
301
        }
302
        break;
303
    case JSButton::button_function_t::k_gain_toggle:
304
        if (!held) {
305
            static bool lowGain = false;
306
            lowGain = !lowGain;
307
            if (lowGain) {
308
                gain = 0.5f;
309
            } else {
310
                gain = 1.0f;
311
            }
312
            gcs().send_text(MAV_SEVERITY_INFO,"#Gain: %2.0f%%",(double)gain*100);
313
        }
314
        break;
315
    case JSButton::button_function_t::k_gain_inc:
316
        if (!held) {
317
            // check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
318
            g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
319
            g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
320
            g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));
321

322
            if (g.numGainSettings == 1) {
323
                gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
324
            } else {
325
                gain = constrain_float(gain + (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
326
            }
327

328
            gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
329
        }
330
        break;
331
    case JSButton::button_function_t::k_gain_dec:
332
        if (!held) {
333
            // check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
334
            g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
335
            g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
336
            g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));
337

338
            if (g.numGainSettings == 1) {
339
                gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
340
            } else {
341
                gain = constrain_float(gain - (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
342
            }
343

344
            gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
345
        }
346
        break;
347
    case JSButton::button_function_t::k_trim_roll_inc:
348
        rollTrim = constrain_float(rollTrim+10,-200,200);
349
        break;
350
    case JSButton::button_function_t::k_trim_roll_dec:
351
        rollTrim = constrain_float(rollTrim-10,-200,200);
352
        break;
353
    case JSButton::button_function_t::k_trim_pitch_inc:
354
        pitchTrim = constrain_float(pitchTrim+10,-200,200);
355
        break;
356
    case JSButton::button_function_t::k_trim_pitch_dec:
357
        pitchTrim = constrain_float(pitchTrim-10,-200,200);
358
        break;
359
    case JSButton::button_function_t::k_input_hold_set:
360
        if(!motors.armed()) {
361
            break;
362
        }
363
        if (!held) {
364
            zTrim = abs(z_last-500) > 50 ? z_last-500 : 0;
365
            xTrim = abs(x_last) > 50 ? x_last : 0;
366
            yTrim = abs(y_last) > 50 ? y_last : 0;
367
            bool input_hold_engaged_last = input_hold_engaged;
368
            input_hold_engaged = zTrim || xTrim || yTrim;
369
            if (input_hold_engaged) {
370
                gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Set");
371
            } else if (input_hold_engaged_last) {
372
                gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Disabled");
373
            }
374
            controls_reset_since_input_hold = !input_hold_engaged;
375
        }
376
        break;
377
#if AP_RELAY_ENABLED
378
    case JSButton::button_function_t::k_relay_1_on:
379
        relay.on(0);
380
        break;
381
    case JSButton::button_function_t::k_relay_1_off:
382
        relay.off(0);
383
        break;
384
    case JSButton::button_function_t::k_relay_1_toggle:
385
        if (!held) {
386
            relay.toggle(0);
387
        }
388
        break;
389
    case JSButton::button_function_t::k_relay_1_momentary:
390
        if (!held) {
391
            relay.on(0);
392
        }
393
        break;
394
    case JSButton::button_function_t::k_relay_2_on:
395
        relay.on(1);
396
        break;
397
    case JSButton::button_function_t::k_relay_2_off:
398
        relay.off(1);
399
        break;
400
    case JSButton::button_function_t::k_relay_2_toggle:
401
        if (!held) {
402
            relay.toggle(1);
403
        }
404
        break;
405
    case JSButton::button_function_t::k_relay_2_momentary:
406
        if (!held) {
407
            relay.on(1);
408
        }
409
        break;
410
    case JSButton::button_function_t::k_relay_3_on:
411
        relay.on(2);
412
        break;
413
    case JSButton::button_function_t::k_relay_3_off:
414
        relay.off(2);
415
        break;
416
    case JSButton::button_function_t::k_relay_3_toggle:
417
        if (!held) {
418
            relay.toggle(2);
419
        }
420
        break;
421
    case JSButton::button_function_t::k_relay_3_momentary:
422
        if (!held) {
423
            relay.on(2);
424
        }
425
        break;
426
    case JSButton::button_function_t::k_relay_4_on:
427
        relay.on(3);
428
        break;
429
    case JSButton::button_function_t::k_relay_4_off:
430
        relay.off(3);
431
        break;
432
    case JSButton::button_function_t::k_relay_4_toggle:
433
        if (!held) {
434
            relay.toggle(3);
435
        }
436
        break;
437
    case JSButton::button_function_t::k_relay_4_momentary:
438
        if (!held) {
439
            relay.on(3);
440
        }
441
        break;
442
#endif
443

444
    ////////////////////////////////////////////////
445
    // Servo functions
446
    // TODO: initialize
447
#if AP_SERVORELAYEVENTS_ENABLED
448
    case JSButton::button_function_t::k_servo_1_inc:
449
    {
450
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
451
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed
452
        pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max());
453
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed
454
    }
455
        break;
456
    case JSButton::button_function_t::k_servo_1_dec:
457
    {
458
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
459
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_1 - 1); // 0-indexed
460
        pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max());
461
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, pwm_out); // 1-indexed
462
    }
463
        break;
464
    case JSButton::button_function_t::k_servo_1_min:
465
    case JSButton::button_function_t::k_servo_1_min_momentary:
466
    {
467
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
468
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_min()); // 1-indexed
469
    }
470
        break;
471
    case JSButton::button_function_t::k_servo_1_min_toggle:
472
        if(!held) {
473
            SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
474
            if(chan->get_output_pwm() != chan->get_output_min()) {
475
                ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_min()); // 1-indexed
476
            } else {
477
                ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed
478
            }
479
        }
480
        break;
481
    case JSButton::button_function_t::k_servo_1_max:
482
    case JSButton::button_function_t::k_servo_1_max_momentary:
483
    {
484
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
485
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_max()); // 1-indexed
486
    }
487
        break;
488
    case JSButton::button_function_t::k_servo_1_max_toggle:
489
        if(!held) {
490
            SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
491
            if(chan->get_output_pwm() != chan->get_output_max()) {
492
                ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_output_max()); // 1-indexed
493
            } else {
494
                ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed
495
            }
496
        }
497
        break;
498
    case JSButton::button_function_t::k_servo_1_center:
499
    {
500
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
501
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed
502
    }
503
        break;
504

505
    case JSButton::button_function_t::k_servo_2_inc:
506
    {
507
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
508
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed
509
        pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max());
510
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed
511
    }
512
        break;
513
    case JSButton::button_function_t::k_servo_2_dec:
514
    {
515
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
516
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_2 - 1); // 0-indexed
517
        pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max());
518
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, pwm_out); // 1-indexed
519
    }
520
        break;
521
    case JSButton::button_function_t::k_servo_2_min:
522
    case JSButton::button_function_t::k_servo_2_min_momentary:
523
    {
524
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
525
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed
526
    }
527
        break;
528
    case JSButton::button_function_t::k_servo_2_min_toggle:
529
        if(!held) {
530
            SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
531
            if(chan->get_output_pwm() != chan->get_output_min()) {
532
                ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_min()); // 1-indexed
533
            } else {
534
                ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed
535
            }
536
        }
537
        break;
538
    case JSButton::button_function_t::k_servo_2_max:
539
    case JSButton::button_function_t::k_servo_2_max_momentary:
540
    {
541
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
542
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed
543
    }
544
        break;
545
    case JSButton::button_function_t::k_servo_2_max_toggle:
546
        if(!held) {
547
            SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
548
            if(chan->get_output_pwm() != chan->get_output_max()) {
549
                ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_output_max()); // 1-indexed
550
            } else {
551
                ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed
552
            }
553
        }
554
        break;
555
    case JSButton::button_function_t::k_servo_2_center:
556
    {
557
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
558
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed
559
    }
560
        break;
561

562
    case JSButton::button_function_t::k_servo_3_inc:
563
    {
564
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
565
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed
566
        pwm_out = constrain_int16(pwm_out + 50, chan->get_output_min(), chan->get_output_max());
567
        ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed
568
    }
569
        break;
570
    case JSButton::button_function_t::k_servo_3_dec:
571
    {
572
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
573
        uint16_t pwm_out = hal.rcout->read(SERVO_CHAN_3 - 1); // 0-indexed
574
        pwm_out = constrain_int16(pwm_out - 50, chan->get_output_min(), chan->get_output_max());
575
        ServoRelayEvents.do_set_servo(SERVO_CHAN_3, pwm_out); // 1-indexed
576
    }
577
        break;
578
    case JSButton::button_function_t::k_servo_3_min:
579
    case JSButton::button_function_t::k_servo_3_min_momentary:
580
    {
581
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
582
        ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_min()); // 1-indexed
583
    }
584
        break;
585
    case JSButton::button_function_t::k_servo_3_min_toggle:
586
        if(!held) {
587
            SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
588
            if(chan->get_output_pwm() != chan->get_output_min()) {
589
                ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_min()); // 1-indexed
590
            } else {
591
                ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed
592
            }
593
        }
594
        break;
595
    case JSButton::button_function_t::k_servo_3_max:
596
    case JSButton::button_function_t::k_servo_3_max_momentary:
597
    {
598
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
599
        ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_max()); // 1-indexed
600
    }
601
        break;
602
    case JSButton::button_function_t::k_servo_3_max_toggle:
603
        if(!held) {
604
            SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
605
            if(chan->get_output_pwm() != chan->get_output_max()) {
606
                ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_output_max()); // 1-indexed
607
            } else {
608
                ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed
609
            }
610
        }
611
        break;
612
    case JSButton::button_function_t::k_servo_3_center:
613
    {
614
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
615
        ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed
616
    }
617
        break;
618
#endif  // AP_SERVORELAYEVENTS_ENABLED
619

620
    case JSButton::button_function_t::k_roll_pitch_toggle:
621
        if (!held) {
622
            roll_pitch_flag = !roll_pitch_flag;
623
            if (roll_pitch_flag) {
624
                gcs().send_text(MAV_SEVERITY_INFO, "#Attitude Control");
625
            }
626
            else {
627
                gcs().send_text(MAV_SEVERITY_INFO, "#Movement Control");
628
            }
629
        }
630
        break;
631

632
    case JSButton::button_function_t::k_custom_1:
633
        // Not implemented
634
        break;
635
    case JSButton::button_function_t::k_custom_2:
636
        // Not implemented
637
        break;
638
    case JSButton::button_function_t::k_custom_3:
639
        // Not implemented
640
        break;
641
    case JSButton::button_function_t::k_custom_4:
642
        // Not implemented
643
        break;
644
    case JSButton::button_function_t::k_custom_5:
645
        // Not implemented
646
        break;
647
    case JSButton::button_function_t::k_custom_6:
648
        // Not implemented
649
        break;
650

651
#if AP_SCRIPTING_ENABLED
652
    case JSButton::button_function_t::k_script_1:
653
        sub.script_buttons[0].press();
654
        break;
655
    case JSButton::button_function_t::k_script_2:
656
        sub.script_buttons[1].press();
657
        break;
658
    case JSButton::button_function_t::k_script_3:
659
        sub.script_buttons[2].press();
660
        break;
661
    case JSButton::button_function_t::k_script_4:
662
        sub.script_buttons[3].press();
663
        break;
664
#endif // AP_SCRIPTING_ENABLED
665
    }
666
}
667

668
void Sub::handle_jsbutton_release(uint8_t _button, bool shift) {
669

670
    // Act based on the function assigned to this button
671
    switch (get_button(_button)->function(shift)) {
672
#if AP_RELAY_ENABLED
673
    case JSButton::button_function_t::k_relay_1_momentary:
674
        relay.off(0);
675
        break;
676
    case JSButton::button_function_t::k_relay_2_momentary:
677
        relay.off(1);
678
        break;
679
    case JSButton::button_function_t::k_relay_3_momentary:
680
        relay.off(2);
681
        break;
682
    case JSButton::button_function_t::k_relay_4_momentary:
683
        relay.off(3);
684
        break;
685
#endif
686
#if AP_SERVORELAYEVENTS_ENABLED
687
    case JSButton::button_function_t::k_servo_1_min_momentary:
688
    case JSButton::button_function_t::k_servo_1_max_momentary:
689
    {
690
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_1 - 1); // 0-indexed
691
        ServoRelayEvents.do_set_servo(SERVO_CHAN_1, chan->get_trim()); // 1-indexed
692
    }
693
        break;
694
    case JSButton::button_function_t::k_servo_2_min_momentary:
695
    case JSButton::button_function_t::k_servo_2_max_momentary:
696
    {
697
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_2 - 1); // 0-indexed
698
        ServoRelayEvents.do_set_servo(SERVO_CHAN_2, chan->get_trim()); // 1-indexed
699
    }
700
        break;
701
    case JSButton::button_function_t::k_servo_3_min_momentary:
702
    case JSButton::button_function_t::k_servo_3_max_momentary:
703
    {
704
        SRV_Channel* chan = SRV_Channels::srv_channel(SERVO_CHAN_3 - 1); // 0-indexed
705
        ServoRelayEvents.do_set_servo(SERVO_CHAN_3, chan->get_trim()); // 1-indexed
706
    }
707
        break;
708
#endif
709

710
#if AP_SCRIPTING_ENABLED
711
    case JSButton::button_function_t::k_script_1:
712
        sub.script_buttons[0].release();
713
        break;
714
    case JSButton::button_function_t::k_script_2:
715
        sub.script_buttons[1].release();
716
        break;
717
    case JSButton::button_function_t::k_script_3:
718
        sub.script_buttons[2].release();
719
        break;
720
    case JSButton::button_function_t::k_script_4:
721
        sub.script_buttons[3].release();
722
        break;
723
#endif // AP_SCRIPTING_ENABLED
724
    }
725
}
726

727
JSButton* Sub::get_button(uint8_t index)
728
{
729
    // Help to access appropriate parameter
730
    switch (index) {
731
    case 0:
732
        return &g.jbtn_0;
733
    case 1:
734
        return &g.jbtn_1;
735
    case 2:
736
        return &g.jbtn_2;
737
    case 3:
738
        return &g.jbtn_3;
739
    case 4:
740
        return &g.jbtn_4;
741
    case 5:
742
        return &g.jbtn_5;
743
    case 6:
744
        return &g.jbtn_6;
745
    case 7:
746
        return &g.jbtn_7;
747
    case 8:
748
        return &g.jbtn_8;
749
    case 9:
750
        return &g.jbtn_9;
751
    case 10:
752
        return &g.jbtn_10;
753
    case 11:
754
        return &g.jbtn_11;
755
    case 12:
756
        return &g.jbtn_12;
757
    case 13:
758
        return &g.jbtn_13;
759
    case 14:
760
        return &g.jbtn_14;
761
    case 15:
762
        return &g.jbtn_15;
763

764
    // add 16 more cases for 32 buttons with MANUAL_CONTROL extensions
765
    case 16:
766
        return &g.jbtn_16;
767
    case 17:
768
        return &g.jbtn_17;
769
    case 18:
770
        return &g.jbtn_18;
771
    case 19:
772
        return &g.jbtn_19;
773
    case 20:
774
        return &g.jbtn_20;
775
    case 21:
776
        return &g.jbtn_21;
777
    case 22:
778
        return &g.jbtn_22;
779
    case 23:
780
        return &g.jbtn_23;
781
    case 24:
782
        return &g.jbtn_24;
783
    case 25:
784
        return &g.jbtn_25;
785
    case 26:
786
        return &g.jbtn_26;
787
    case 27:
788
        return &g.jbtn_27;
789
    case 28:
790
        return &g.jbtn_28;
791
    case 29:
792
        return &g.jbtn_29;
793
    case 30:
794
        return &g.jbtn_30;
795
    case 31:
796
        return &g.jbtn_31;
797
    default:
798
        return &g.jbtn_0;
799
    }
800
}
801

802
void Sub::default_js_buttons()
803
{
804
    JSButton::button_function_t defaults[16][2] = {
805
        {JSButton::button_function_t::k_none,                   JSButton::button_function_t::k_none},
806
        {JSButton::button_function_t::k_mode_manual,            JSButton::button_function_t::k_none},
807
        {JSButton::button_function_t::k_mode_depth_hold,        JSButton::button_function_t::k_none},
808
        {JSButton::button_function_t::k_mode_stabilize,         JSButton::button_function_t::k_none},
809

810
        {JSButton::button_function_t::k_disarm,                 JSButton::button_function_t::k_none},
811
        {JSButton::button_function_t::k_shift,                  JSButton::button_function_t::k_none},
812
        {JSButton::button_function_t::k_arm,                    JSButton::button_function_t::k_none},
813
        {JSButton::button_function_t::k_mount_center,           JSButton::button_function_t::k_none},
814

815
        {JSButton::button_function_t::k_input_hold_set,         JSButton::button_function_t::k_none},
816
        {JSButton::button_function_t::k_mount_tilt_down,        JSButton::button_function_t::k_mount_pan_left},
817
        {JSButton::button_function_t::k_mount_tilt_up,          JSButton::button_function_t::k_mount_pan_right},
818
        {JSButton::button_function_t::k_gain_inc,               JSButton::button_function_t::k_trim_pitch_dec},
819

820
        {JSButton::button_function_t::k_gain_dec,               JSButton::button_function_t::k_trim_pitch_inc},
821
        {JSButton::button_function_t::k_lights1_dimmer,         JSButton::button_function_t::k_trim_roll_dec},
822
        {JSButton::button_function_t::k_lights1_brighter,       JSButton::button_function_t::k_trim_roll_inc},
823
        {JSButton::button_function_t::k_none,                   JSButton::button_function_t::k_none},
824
    };
825

826
    for (int i = 0; i < 16; i++) {
827
        get_button(i)->set_default(defaults[i][0], defaults[i][1]);
828
    }
829
}
830

831
void Sub::set_neutral_controls()
832
{
833
    uint32_t tnow = AP_HAL::millis();
834

835
    for (uint8_t i = 0; i < 6; i++) {
836
        RC_Channels::set_override(i, 1500, tnow);
837
    }
838

839
    // Clear pitch/roll trim settings
840
    pitchTrim = 0;
841
    rollTrim  = 0;
842
}
843

844
void Sub::clear_input_hold()
845
{
846
    xTrim = 0;
847
    yTrim = 0;
848
    zTrim = 0;
849
    input_hold_engaged = false;
850
}
851

852
#if AP_SCRIPTING_ENABLED
853
bool Sub::is_button_pressed(uint8_t index)
854
{
855
    return script_buttons[index - 1].is_pressed();
856
}
857

858
uint8_t Sub::get_and_clear_button_count(uint8_t index)
859
{
860
    return script_buttons[index - 1].get_and_clear_count();
861
}
862
#endif // AP_SCRIPTING_ENABLED
863

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