Line data Source code
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 : #if HAL_MOUNT_ENABLED
10 : float cam_tilt = 1500.0;
11 : float cam_pan = 1500.0;
12 : #endif // HAL_MOUNT_ENABLED
13 : float lights1 = 0;
14 : float lights2 = 0;
15 : int16_t rollTrim = 0;
16 : int16_t pitchTrim = 0;
17 : int16_t zTrim = 0;
18 : int16_t xTrim = 0;
19 : int16_t yTrim = 0;
20 : int16_t x_last, y_last, z_last;
21 : uint32_t buttons_prev;
22 :
23 : bool controls_reset_since_input_hold = true;
24 : }
25 :
26 83 : void Sub::init_joystick()
27 : {
28 83 : default_js_buttons();
29 :
30 83 : set_mode(Mode::Number::MANUAL, ModeReason::RC_COMMAND); // Initialize flight mode
31 :
32 83 : if (g.numGainSettings < 1) {
33 0 : g.numGainSettings.set_and_save(1);
34 : }
35 :
36 83 : if (g.numGainSettings == 1 || (g.gain_default < g.maxGain + 0.01 && g.gain_default > g.minGain - 0.01)) {
37 83 : gain = constrain_float(g.gain_default, g.minGain, g.maxGain); // Use default gain parameter
38 : } else {
39 : // Use setting closest to average of minGain and maxGain
40 0 : gain = g.minGain + (g.numGainSettings/2 - 1) * (g.maxGain - g.minGain) / (g.numGainSettings - 1);
41 : }
42 :
43 83 : gain = constrain_float(gain, 0.1, 1.0);
44 83 : SRV_Channels::set_output_scaled(SRV_Channel::k_lights1, 0.0);
45 83 : SRV_Channels::set_output_scaled(SRV_Channel::k_lights2, 0.0);
46 83 : SRV_Channels::set_output_scaled(SRV_Channel::k_video_switch, 0.0);
47 83 : }
48 :
49 0 : 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,
50 : int16_t s,
51 : int16_t t,
52 : int16_t aux1,
53 : int16_t aux2,
54 : int16_t aux3,
55 : int16_t aux4,
56 : int16_t aux5,
57 : int16_t aux6)
58 : {
59 :
60 0 : float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain
61 0 : float throttleScale = 0.8*gain*g.throttle_gain; // Scale 0-1000 to 0-800 times gain
62 0 : int16_t rpyCenter = 1500;
63 0 : int16_t throttleBase = 1500-500*throttleScale;
64 :
65 0 : bool shift = false;
66 :
67 : #if HAL_MOUNT_ENABLED
68 : // Neutralize camera tilt and pan speed setpoint
69 0 : cam_tilt = 1500;
70 0 : cam_pan = 1500;
71 : #endif // HAL_MOUNT_ENABLED
72 :
73 0 : uint32_t all_buttons = buttons | (buttons2 << 16);
74 : // Detect if any shift button is pressed
75 0 : for (uint8_t i = 0 ; i < 32 ; i++) {
76 0 : if ((all_buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) {
77 0 : shift = true;
78 : }
79 : }
80 :
81 : // Act if button is pressed
82 : // Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick.
83 0 : for (uint8_t i = 0 ; i < 32 ; i++) {
84 0 : if ((all_buttons & (1 << i))) {
85 0 : handle_jsbutton_press(i,shift,(buttons_prev & (1 << i)));
86 : // buttonDebounce = tnow_ms;
87 0 : } else if (buttons_prev & (1 << i)) {
88 0 : handle_jsbutton_release(i, shift);
89 : }
90 : }
91 :
92 0 : buttons_prev = all_buttons;
93 :
94 : // attitude mode:
95 0 : if (roll_pitch_flag == 1) {
96 : // adjust roll/pitch trim with joystick input instead of forward/lateral
97 0 : pitchTrim = -x * rpyScale;
98 0 : rollTrim = y * rpyScale;
99 : }
100 :
101 0 : uint32_t tnow = AP_HAL::millis();
102 :
103 : int16_t zTot;
104 : int16_t yTot;
105 : int16_t xTot;
106 :
107 0 : if (!controls_reset_since_input_hold) {
108 0 : zTot = zTrim + 500; // 500 is neutral for throttle
109 0 : yTot = yTrim;
110 0 : xTot = xTrim;
111 : // if all 3 axes return to neutral, than we're ready to accept input again
112 0 : controls_reset_since_input_hold = (abs(z - 500) < 50) && (abs(y) < 50) && (abs(x) < 50);
113 : } else {
114 0 : zTot = z + zTrim;
115 0 : yTot = y + yTrim;
116 0 : xTot = x + xTrim;
117 : }
118 :
119 0 : channel_pitch->set_override(constrain_int16(s + pitchTrim + rpyCenter,1100,1900), tnow);
120 0 : channel_roll->set_override(constrain_int16(t + rollTrim + rpyCenter,1100,1900), tnow);
121 :
122 0 : channel_throttle->set_override(constrain_int16((zTot)*throttleScale+throttleBase,1100,1900), tnow);
123 0 : channel_yaw->set_override(constrain_int16(r*rpyScale+rpyCenter,1100,1900), tnow);
124 :
125 : // maneuver mode:
126 0 : if (roll_pitch_flag == 0) {
127 : // adjust forward and lateral with joystick input instead of roll and pitch
128 0 : channel_forward->set_override(constrain_int16((xTot)*rpyScale+rpyCenter,1100,1900), tnow);
129 0 : channel_lateral->set_override(constrain_int16((yTot)*rpyScale+rpyCenter,1100,1900), tnow);
130 : } else {
131 : // neutralize forward and lateral input while we are adjusting roll and pitch
132 0 : channel_forward->set_override(constrain_int16(xTrim*rpyScale+rpyCenter,1100,1900), tnow);
133 0 : channel_lateral->set_override(constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900), tnow);
134 : }
135 :
136 : #if HAL_MOUNT_ENABLED
137 0 : RC_Channel *cam_pan_chan = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOUNT1_YAW);
138 0 : if (cam_pan_chan != nullptr) {
139 0 : cam_pan_chan->set_override(cam_pan, tnow);
140 : }
141 0 : RC_Channel *cam_tilt_chan = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOUNT1_PITCH);
142 0 : if (cam_tilt_chan != nullptr) {
143 0 : cam_tilt_chan->set_override(cam_tilt, tnow);
144 : }
145 : #endif // HAL_MOUNT_ENABLED
146 :
147 : // Store old x, y, z values for use in input hold logic
148 0 : x_last = x;
149 0 : y_last = y;
150 0 : z_last = z;
151 0 : }
152 :
153 0 : void Sub::handle_jsbutton_press(uint8_t _button, bool shift, bool held)
154 : {
155 : // Act based on the function assigned to this button
156 0 : switch (get_button(_button)->function(shift)) {
157 0 : case JSButton::button_function_t::k_arm_toggle:
158 0 : if (motors.armed()) {
159 0 : arming.disarm(AP_Arming::Method::MAVLINK);
160 : } else {
161 0 : arming.arm(AP_Arming::Method::MAVLINK);
162 : }
163 0 : break;
164 0 : case JSButton::button_function_t::k_arm:
165 0 : arming.arm(AP_Arming::Method::MAVLINK);
166 0 : break;
167 0 : case JSButton::button_function_t::k_disarm:
168 0 : arming.disarm(AP_Arming::Method::MAVLINK);
169 0 : break;
170 :
171 0 : case JSButton::button_function_t::k_mode_manual:
172 0 : set_mode(Mode::Number::MANUAL, ModeReason::RC_COMMAND);
173 0 : break;
174 0 : case JSButton::button_function_t::k_mode_stabilize:
175 0 : set_mode(Mode::Number::STABILIZE, ModeReason::RC_COMMAND);
176 0 : break;
177 0 : case JSButton::button_function_t::k_mode_depth_hold:
178 0 : set_mode(Mode::Number::ALT_HOLD, ModeReason::RC_COMMAND);
179 0 : break;
180 0 : case JSButton::button_function_t::k_mode_auto:
181 0 : set_mode(Mode::Number::AUTO, ModeReason::RC_COMMAND);
182 0 : break;
183 0 : case JSButton::button_function_t::k_mode_guided:
184 0 : set_mode(Mode::Number::GUIDED, ModeReason::RC_COMMAND);
185 0 : break;
186 0 : case JSButton::button_function_t::k_mode_circle:
187 0 : set_mode(Mode::Number::CIRCLE, ModeReason::RC_COMMAND);
188 0 : break;
189 0 : case JSButton::button_function_t::k_mode_acro:
190 0 : set_mode(Mode::Number::ACRO, ModeReason::RC_COMMAND);
191 0 : break;
192 0 : case JSButton::button_function_t::k_mode_poshold:
193 0 : set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND);
194 0 : break;
195 : #if AP_RANGEFINDER_ENABLED
196 0 : case JSButton::button_function_t::k_mode_surftrak:
197 0 : set_mode(Mode::Number::SURFTRAK, ModeReason::RC_COMMAND);
198 0 : break;
199 : #endif
200 : #if HAL_MOUNT_ENABLED
201 0 : case JSButton::button_function_t::k_mount_center:
202 0 : camera_mount.set_angle_target(0, 0, 0, false);
203 : // for some reason the call to set_angle_targets changes the mode to mavlink targeting!
204 0 : camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING);
205 0 : break;
206 0 : case JSButton::button_function_t::k_mount_tilt_up:
207 0 : cam_tilt = 1900;
208 0 : break;
209 0 : case JSButton::button_function_t::k_mount_tilt_down:
210 0 : cam_tilt = 1100;
211 0 : break;
212 : #endif // HAL_MOUNT_ENABLED
213 0 : case JSButton::button_function_t::k_camera_trigger:
214 0 : break;
215 0 : case JSButton::button_function_t::k_camera_source_toggle:
216 0 : if (!held) {
217 : static bool video_toggle = false;
218 0 : video_toggle = !video_toggle;
219 0 : if (video_toggle) {
220 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_video_switch, 1000);
221 0 : gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 2");
222 : } else {
223 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_video_switch, 0.0);
224 0 : gcs().send_text(MAV_SEVERITY_INFO,"Video Toggle: Source 1");
225 : }
226 : }
227 0 : break;
228 : #if HAL_MOUNT_ENABLED
229 0 : case JSButton::button_function_t::k_mount_pan_right:
230 0 : cam_pan = 1900;
231 0 : break;
232 0 : case JSButton::button_function_t::k_mount_pan_left:
233 0 : cam_pan = 1100;
234 0 : break;
235 : #endif // HAL_MOUNT_ENABLED
236 0 : case JSButton::button_function_t::k_lights1_cycle:
237 0 : if (!held) {
238 : static bool increasing = true;
239 0 : uint16_t step = 1000.0 / g.lights_steps;
240 0 : if (increasing) {
241 0 : lights1 = constrain_float(lights1 + step, 0.0, 1000.0);
242 : } else {
243 0 : lights1 = constrain_float(lights1 - step, 0.0, 1000.0);
244 : }
245 0 : if (lights1 >= 1000.0 || lights1 <= 0.0) {
246 0 : increasing = !increasing;
247 : }
248 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_lights1, lights1);
249 : }
250 0 : break;
251 0 : case JSButton::button_function_t::k_lights1_brighter:
252 0 : if (!held) {
253 0 : uint16_t step = 1000.0 / g.lights_steps;
254 0 : lights1 = constrain_float(lights1 + step, 0.0, 1000.0);
255 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_lights1, lights1);
256 : }
257 0 : break;
258 0 : case JSButton::button_function_t::k_lights1_dimmer:
259 0 : if (!held) {
260 0 : uint16_t step = 1000.0 / g.lights_steps;
261 0 : lights1 = constrain_float(lights1 - step, 0.0, 1000.0);
262 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_lights1, lights1);
263 : }
264 0 : break;
265 0 : case JSButton::button_function_t::k_lights2_cycle:
266 0 : if (!held) {
267 : static bool increasing = true;
268 0 : uint16_t step = 1000.0 / g.lights_steps;
269 0 : if (increasing) {
270 0 : lights2 = constrain_float(lights2 + step, 0.0, 1000.0);
271 : } else {
272 0 : lights2 = constrain_float(lights2 - step, 0.0, 1000.0);
273 : }
274 0 : if (lights2 >= 1000.0 || lights2 <= 0.0) {
275 0 : increasing = !increasing;
276 : }
277 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_lights2, lights2);
278 : }
279 0 : break;
280 0 : case JSButton::button_function_t::k_lights2_brighter:
281 0 : if (!held) {
282 0 : uint16_t step = 1000.0 / g.lights_steps;
283 0 : lights2 = constrain_float(lights2 + step, 0.0, 1000.0);
284 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_lights2, lights2);
285 : }
286 0 : break;
287 0 : case JSButton::button_function_t::k_lights2_dimmer:
288 0 : if (!held) {
289 0 : uint16_t step = 1000.0 / g.lights_steps;
290 0 : lights2 = constrain_float(lights2 - step, 0.0, 1000.0);
291 0 : SRV_Channels::set_output_scaled(SRV_Channel::k_lights2, lights2);
292 : }
293 0 : break;
294 0 : case JSButton::button_function_t::k_gain_toggle:
295 0 : if (!held) {
296 : static bool lowGain = false;
297 0 : lowGain = !lowGain;
298 0 : if (lowGain) {
299 0 : gain = 0.5f;
300 : } else {
301 0 : gain = 1.0f;
302 : }
303 0 : gcs().send_text(MAV_SEVERITY_INFO,"#Gain: %2.0f%%",(double)gain*100);
304 : }
305 0 : break;
306 0 : case JSButton::button_function_t::k_gain_inc:
307 0 : if (!held) {
308 : // check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
309 0 : g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
310 0 : g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
311 0 : g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));
312 :
313 0 : if (g.numGainSettings == 1) {
314 0 : gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
315 : } else {
316 0 : gain = constrain_float(gain + (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
317 : }
318 :
319 0 : gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
320 : }
321 0 : break;
322 0 : case JSButton::button_function_t::k_gain_dec:
323 0 : if (!held) {
324 : // check that our gain parameters are in correct range, update in eeprom and notify gcs if needed
325 0 : g.minGain.set_and_save(constrain_float(g.minGain, 0.10, 0.80));
326 0 : g.maxGain.set_and_save(constrain_float(g.maxGain, g.minGain, 1.0));
327 0 : g.numGainSettings.set_and_save(constrain_int16(g.numGainSettings, 1, 10));
328 :
329 0 : if (g.numGainSettings == 1) {
330 0 : gain = constrain_float(g.gain_default, g.minGain, g.maxGain);
331 : } else {
332 0 : gain = constrain_float(gain - (g.maxGain-g.minGain)/(g.numGainSettings-1), g.minGain, g.maxGain);
333 : }
334 :
335 0 : gcs().send_text(MAV_SEVERITY_INFO,"#Gain is %2.0f%%",(double)gain*100);
336 : }
337 0 : break;
338 0 : case JSButton::button_function_t::k_trim_roll_inc:
339 0 : rollTrim = constrain_float(rollTrim+10,-200,200);
340 0 : break;
341 0 : case JSButton::button_function_t::k_trim_roll_dec:
342 0 : rollTrim = constrain_float(rollTrim-10,-200,200);
343 0 : break;
344 0 : case JSButton::button_function_t::k_trim_pitch_inc:
345 0 : pitchTrim = constrain_float(pitchTrim+10,-200,200);
346 0 : break;
347 0 : case JSButton::button_function_t::k_trim_pitch_dec:
348 0 : pitchTrim = constrain_float(pitchTrim-10,-200,200);
349 0 : break;
350 0 : case JSButton::button_function_t::k_input_hold_set:
351 0 : if(!motors.armed()) {
352 0 : break;
353 : }
354 0 : if (!held) {
355 0 : zTrim = abs(z_last-500) > 50 ? z_last-500 : 0;
356 0 : xTrim = abs(x_last) > 50 ? x_last : 0;
357 0 : yTrim = abs(y_last) > 50 ? y_last : 0;
358 0 : bool input_hold_engaged_last = input_hold_engaged;
359 0 : input_hold_engaged = zTrim || xTrim || yTrim;
360 0 : if (input_hold_engaged) {
361 0 : gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Set");
362 0 : } else if (input_hold_engaged_last) {
363 0 : gcs().send_text(MAV_SEVERITY_INFO,"#Input Hold Disabled");
364 : }
365 0 : controls_reset_since_input_hold = !input_hold_engaged;
366 : }
367 0 : break;
368 : #if AP_RELAY_ENABLED
369 0 : case JSButton::button_function_t::k_relay_1_on:
370 0 : relay.on(0);
371 0 : break;
372 0 : case JSButton::button_function_t::k_relay_1_off:
373 0 : relay.off(0);
374 0 : break;
375 0 : case JSButton::button_function_t::k_relay_1_toggle:
376 0 : if (!held) {
377 0 : relay.toggle(0);
378 : }
379 0 : break;
380 0 : case JSButton::button_function_t::k_relay_1_momentary:
381 0 : if (!held) {
382 0 : relay.on(0);
383 : }
384 0 : break;
385 0 : case JSButton::button_function_t::k_relay_2_on:
386 0 : relay.on(1);
387 0 : break;
388 0 : case JSButton::button_function_t::k_relay_2_off:
389 0 : relay.off(1);
390 0 : break;
391 0 : case JSButton::button_function_t::k_relay_2_toggle:
392 0 : if (!held) {
393 0 : relay.toggle(1);
394 : }
395 0 : break;
396 0 : case JSButton::button_function_t::k_relay_2_momentary:
397 0 : if (!held) {
398 0 : relay.on(1);
399 : }
400 0 : break;
401 0 : case JSButton::button_function_t::k_relay_3_on:
402 0 : relay.on(2);
403 0 : break;
404 0 : case JSButton::button_function_t::k_relay_3_off:
405 0 : relay.off(2);
406 0 : break;
407 0 : case JSButton::button_function_t::k_relay_3_toggle:
408 0 : if (!held) {
409 0 : relay.toggle(2);
410 : }
411 0 : break;
412 0 : case JSButton::button_function_t::k_relay_3_momentary:
413 0 : if (!held) {
414 0 : relay.on(2);
415 : }
416 0 : break;
417 0 : case JSButton::button_function_t::k_relay_4_on:
418 0 : relay.on(3);
419 0 : break;
420 0 : case JSButton::button_function_t::k_relay_4_off:
421 0 : relay.off(3);
422 0 : break;
423 0 : case JSButton::button_function_t::k_relay_4_toggle:
424 0 : if (!held) {
425 0 : relay.toggle(3);
426 : }
427 0 : break;
428 0 : case JSButton::button_function_t::k_relay_4_momentary:
429 0 : if (!held) {
430 0 : relay.on(3);
431 : }
432 0 : break;
433 : #endif
434 :
435 : ////////////////////////////////////////////////
436 : // Servo functions
437 : #if AP_SERVORELAYEVENTS_ENABLED
438 0 : case JSButton::button_function_t::k_servo_1_inc:
439 0 : sub.g2.actuators.increase_actuator(0);
440 0 : break;
441 0 : case JSButton::button_function_t::k_servo_2_inc:
442 0 : sub.g2.actuators.increase_actuator(1);
443 0 : break;
444 0 : case JSButton::button_function_t::k_servo_3_inc:
445 0 : sub.g2.actuators.increase_actuator(2);
446 0 : break;
447 0 : case JSButton::button_function_t::k_servo_4_inc:
448 0 : sub.g2.actuators.increase_actuator(3);
449 0 : break;
450 0 : case JSButton::button_function_t::k_servo_5_inc:
451 0 : sub.g2.actuators.increase_actuator(4);
452 0 : break;
453 0 : case JSButton::button_function_t::k_servo_6_inc:
454 0 : sub.g2.actuators.increase_actuator(5);
455 0 : break;
456 :
457 0 : case JSButton::button_function_t::k_servo_1_dec:
458 0 : sub.g2.actuators.decrease_actuator(0);
459 0 : break;
460 0 : case JSButton::button_function_t::k_servo_2_dec:
461 0 : sub.g2.actuators.decrease_actuator(1);
462 0 : break;
463 0 : case JSButton::button_function_t::k_servo_3_dec:
464 0 : sub.g2.actuators.decrease_actuator(2);
465 0 : break;
466 0 : case JSButton::button_function_t::k_servo_4_dec:
467 0 : sub.g2.actuators.decrease_actuator(3);
468 0 : break;
469 0 : case JSButton::button_function_t::k_servo_5_dec:
470 0 : sub.g2.actuators.decrease_actuator(4);
471 0 : break;
472 0 : case JSButton::button_function_t::k_servo_6_dec:
473 0 : sub.g2.actuators.decrease_actuator(5);
474 0 : break;
475 :
476 0 : case JSButton::button_function_t::k_servo_1_min:
477 : case JSButton::button_function_t::k_servo_1_min_momentary:
478 0 : sub.g2.actuators.min_actuator(0);
479 0 : break;
480 0 : case JSButton::button_function_t::k_servo_2_min:
481 : case JSButton::button_function_t::k_servo_2_min_momentary:
482 0 : sub.g2.actuators.min_actuator(1);
483 0 : break;
484 0 : case JSButton::button_function_t::k_servo_3_min:
485 : case JSButton::button_function_t::k_servo_3_min_momentary:
486 0 : sub.g2.actuators.min_actuator(2);
487 0 : break;
488 0 : case JSButton::button_function_t::k_servo_4_min:
489 : case JSButton::button_function_t::k_servo_4_min_momentary:
490 0 : sub.g2.actuators.min_actuator(3);
491 0 : break;
492 0 : case JSButton::button_function_t::k_servo_5_min:
493 : case JSButton::button_function_t::k_servo_5_min_momentary:
494 0 : sub.g2.actuators.min_actuator(4);
495 0 : break;
496 0 : case JSButton::button_function_t::k_servo_6_min:
497 : case JSButton::button_function_t::k_servo_6_min_momentary:
498 0 : sub.g2.actuators.min_actuator(5);
499 0 : break;
500 :
501 0 : case JSButton::button_function_t::k_servo_1_min_toggle:
502 0 : if (!held) {
503 0 : sub.g2.actuators.min_toggle_actuator(0);
504 : }
505 0 : break;
506 0 : case JSButton::button_function_t::k_servo_2_min_toggle:
507 0 : if (!held) {
508 0 : sub.g2.actuators.min_toggle_actuator(1);
509 : }
510 0 : break;
511 0 : case JSButton::button_function_t::k_servo_3_min_toggle:
512 0 : if (!held) {
513 0 : sub.g2.actuators.min_toggle_actuator(2);
514 : }
515 0 : break;
516 0 : case JSButton::button_function_t::k_servo_4_min_toggle:
517 0 : if (!held) {
518 0 : sub.g2.actuators.min_toggle_actuator(3);
519 : }
520 0 : break;
521 0 : case JSButton::button_function_t::k_servo_5_min_toggle:
522 0 : if (!held) {
523 0 : sub.g2.actuators.min_toggle_actuator(4);
524 : }
525 0 : break;
526 0 : case JSButton::button_function_t::k_servo_6_min_toggle:
527 0 : if (!held) {
528 0 : sub.g2.actuators.min_toggle_actuator(5);
529 : }
530 0 : break;
531 :
532 0 : case JSButton::button_function_t::k_servo_1_max:
533 : case JSButton::button_function_t::k_servo_1_max_momentary:
534 0 : sub.g2.actuators.max_actuator(0);
535 0 : break;
536 0 : case JSButton::button_function_t::k_servo_2_max:
537 : case JSButton::button_function_t::k_servo_2_max_momentary:
538 0 : sub.g2.actuators.max_actuator(1);
539 0 : break;
540 0 : case JSButton::button_function_t::k_servo_3_max:
541 : case JSButton::button_function_t::k_servo_3_max_momentary:
542 0 : sub.g2.actuators.max_actuator(2);
543 0 : break;
544 0 : case JSButton::button_function_t::k_servo_4_max:
545 : case JSButton::button_function_t::k_servo_4_max_momentary:
546 0 : sub.g2.actuators.max_actuator(3);
547 0 : break;
548 0 : case JSButton::button_function_t::k_servo_5_max:
549 : case JSButton::button_function_t::k_servo_5_max_momentary:
550 0 : sub.g2.actuators.max_actuator(4);
551 0 : break;
552 :
553 0 : case JSButton::button_function_t::k_servo_1_max_toggle:
554 0 : if (!held) {
555 0 : sub.g2.actuators.max_toggle_actuator(0);
556 : }
557 0 : break;
558 0 : case JSButton::button_function_t::k_servo_2_max_toggle:
559 0 : if (!held) {
560 0 : sub.g2.actuators.max_toggle_actuator(1);
561 : }
562 0 : break;
563 0 : case JSButton::button_function_t::k_servo_3_max_toggle:
564 0 : if (!held) {
565 0 : sub.g2.actuators.max_toggle_actuator(2);
566 : }
567 0 : break;
568 0 : case JSButton::button_function_t::k_servo_4_max_toggle:
569 0 : if (!held) {
570 0 : sub.g2.actuators.max_toggle_actuator(3);
571 : }
572 0 : break;
573 0 : case JSButton::button_function_t::k_servo_5_max_toggle:
574 0 : if (!held) {
575 0 : sub.g2.actuators.max_toggle_actuator(4);
576 : }
577 0 : break;
578 0 : case JSButton::button_function_t::k_servo_6_max_toggle:
579 0 : if (!held) {
580 0 : sub.g2.actuators.max_toggle_actuator(5);
581 : }
582 0 : break;
583 :
584 0 : case JSButton::button_function_t::k_servo_1_center:
585 0 : sub.g2.actuators.center_actuator(0);
586 0 : break;
587 0 : case JSButton::button_function_t::k_servo_2_center:
588 0 : sub.g2.actuators.center_actuator(1);
589 0 : break;
590 0 : case JSButton::button_function_t::k_servo_3_center:
591 0 : sub.g2.actuators.center_actuator(2);
592 0 : break;
593 0 : case JSButton::button_function_t::k_servo_4_center:
594 0 : sub.g2.actuators.center_actuator(3);
595 0 : break;
596 0 : case JSButton::button_function_t::k_servo_5_center:
597 0 : sub.g2.actuators.center_actuator(4);
598 0 : break;
599 0 : case JSButton::button_function_t::k_servo_6_center:
600 0 : sub.g2.actuators.center_actuator(5);
601 0 : break;
602 : #endif // AP_SERVORELAYEVENTS_ENABLED
603 :
604 0 : case JSButton::button_function_t::k_roll_pitch_toggle:
605 0 : if (!held) {
606 0 : roll_pitch_flag = !roll_pitch_flag;
607 0 : if (roll_pitch_flag) {
608 0 : gcs().send_text(MAV_SEVERITY_INFO, "#Attitude Control");
609 : }
610 : else {
611 0 : gcs().send_text(MAV_SEVERITY_INFO, "#Movement Control");
612 : }
613 : }
614 0 : break;
615 :
616 0 : case JSButton::button_function_t::k_custom_1:
617 : // Not implemented
618 0 : break;
619 0 : case JSButton::button_function_t::k_custom_2:
620 : // Not implemented
621 0 : break;
622 0 : case JSButton::button_function_t::k_custom_3:
623 : // Not implemented
624 0 : break;
625 0 : case JSButton::button_function_t::k_custom_4:
626 : // Not implemented
627 0 : break;
628 0 : case JSButton::button_function_t::k_custom_5:
629 : // Not implemented
630 0 : break;
631 0 : case JSButton::button_function_t::k_custom_6:
632 : // Not implemented
633 0 : break;
634 :
635 : #if AP_SCRIPTING_ENABLED
636 0 : case JSButton::button_function_t::k_script_1:
637 0 : sub.script_buttons[0].press();
638 0 : break;
639 0 : case JSButton::button_function_t::k_script_2:
640 0 : sub.script_buttons[1].press();
641 0 : break;
642 0 : case JSButton::button_function_t::k_script_3:
643 0 : sub.script_buttons[2].press();
644 0 : break;
645 0 : case JSButton::button_function_t::k_script_4:
646 0 : sub.script_buttons[3].press();
647 0 : break;
648 : #endif // AP_SCRIPTING_ENABLED
649 : }
650 0 : }
651 :
652 0 : void Sub::handle_jsbutton_release(uint8_t _button, bool shift) {
653 :
654 : // Act based on the function assigned to this button
655 0 : switch (get_button(_button)->function(shift)) {
656 : #if AP_RELAY_ENABLED
657 0 : case JSButton::button_function_t::k_relay_1_momentary:
658 0 : relay.off(0);
659 0 : break;
660 0 : case JSButton::button_function_t::k_relay_2_momentary:
661 0 : relay.off(1);
662 0 : break;
663 0 : case JSButton::button_function_t::k_relay_3_momentary:
664 0 : relay.off(2);
665 0 : break;
666 0 : case JSButton::button_function_t::k_relay_4_momentary:
667 0 : relay.off(3);
668 0 : break;
669 : #endif
670 : #if AP_SERVORELAYEVENTS_ENABLED
671 0 : case JSButton::button_function_t::k_servo_1_min_momentary:
672 : case JSButton::button_function_t::k_servo_1_max_momentary:
673 : {
674 0 : sub.g2.actuators.center_actuator(0);
675 : }
676 0 : break;
677 0 : case JSButton::button_function_t::k_servo_2_min_momentary:
678 : case JSButton::button_function_t::k_servo_2_max_momentary:
679 : {
680 0 : sub.g2.actuators.center_actuator(1);
681 : }
682 0 : break;
683 0 : case JSButton::button_function_t::k_servo_3_min_momentary:
684 : case JSButton::button_function_t::k_servo_3_max_momentary:
685 : {
686 0 : sub.g2.actuators.center_actuator(2);
687 : }
688 0 : break;
689 0 : case JSButton::button_function_t::k_servo_4_min_momentary:
690 : case JSButton::button_function_t::k_servo_4_max_momentary:
691 : {
692 0 : sub.g2.actuators.center_actuator(3);
693 : }
694 0 : break;
695 0 : case JSButton::button_function_t::k_servo_5_min_momentary:
696 : case JSButton::button_function_t::k_servo_5_max_momentary:
697 : {
698 0 : sub.g2.actuators.center_actuator(4);
699 : }
700 0 : break;
701 0 : case JSButton::button_function_t::k_servo_6_min_momentary:
702 : case JSButton::button_function_t::k_servo_6_max_momentary:
703 : {
704 0 : sub.g2.actuators.center_actuator(5);
705 : }
706 0 : break;
707 : #endif
708 :
709 : #if AP_SCRIPTING_ENABLED
710 0 : case JSButton::button_function_t::k_script_1:
711 0 : sub.script_buttons[0].release();
712 0 : break;
713 0 : case JSButton::button_function_t::k_script_2:
714 0 : sub.script_buttons[1].release();
715 0 : break;
716 0 : case JSButton::button_function_t::k_script_3:
717 0 : sub.script_buttons[2].release();
718 0 : break;
719 0 : case JSButton::button_function_t::k_script_4:
720 0 : sub.script_buttons[3].release();
721 0 : break;
722 : #endif // AP_SCRIPTING_ENABLED
723 : }
724 0 : }
725 :
726 177376 : JSButton* Sub::get_button(uint8_t index)
727 : {
728 : // Help to access appropriate parameter
729 177376 : switch (index) {
730 9540 : case 0:
731 9540 : return &g.jbtn_0;
732 9540 : case 1:
733 9540 : return &g.jbtn_1;
734 9540 : case 2:
735 9540 : return &g.jbtn_2;
736 9540 : case 3:
737 9540 : return &g.jbtn_3;
738 9540 : case 4:
739 9540 : return &g.jbtn_4;
740 4852 : case 5:
741 4852 : return &g.jbtn_5;
742 4852 : case 6:
743 4852 : return &g.jbtn_6;
744 4852 : case 7:
745 4852 : return &g.jbtn_7;
746 4852 : case 8:
747 4852 : return &g.jbtn_8;
748 4852 : case 9:
749 4852 : return &g.jbtn_9;
750 4852 : case 10:
751 4852 : return &g.jbtn_10;
752 4852 : case 11:
753 4852 : return &g.jbtn_11;
754 4852 : case 12:
755 4852 : return &g.jbtn_12;
756 4852 : case 13:
757 4852 : return &g.jbtn_13;
758 4852 : case 14:
759 4852 : return &g.jbtn_14;
760 4852 : case 15:
761 4852 : return &g.jbtn_15;
762 :
763 : // add 16 more cases for 32 buttons with MANUAL_CONTROL extensions
764 4769 : case 16:
765 4769 : return &g.jbtn_16;
766 4769 : case 17:
767 4769 : return &g.jbtn_17;
768 4769 : case 18:
769 4769 : return &g.jbtn_18;
770 4769 : case 19:
771 4769 : return &g.jbtn_19;
772 4769 : case 20:
773 4769 : return &g.jbtn_20;
774 4769 : case 21:
775 4769 : return &g.jbtn_21;
776 4769 : case 22:
777 4769 : return &g.jbtn_22;
778 4769 : case 23:
779 4769 : return &g.jbtn_23;
780 4769 : case 24:
781 4769 : return &g.jbtn_24;
782 4769 : case 25:
783 4769 : return &g.jbtn_25;
784 4769 : case 26:
785 4769 : return &g.jbtn_26;
786 4769 : case 27:
787 4769 : return &g.jbtn_27;
788 4769 : case 28:
789 4769 : return &g.jbtn_28;
790 4769 : case 29:
791 4769 : return &g.jbtn_29;
792 4769 : case 30:
793 4769 : return &g.jbtn_30;
794 4769 : case 31:
795 4769 : return &g.jbtn_31;
796 0 : default:
797 0 : return &g.jbtn_0;
798 : }
799 : }
800 :
801 83 : void Sub::default_js_buttons()
802 : {
803 83 : JSButton::button_function_t defaults[16][2] = {
804 : {JSButton::button_function_t::k_none, JSButton::button_function_t::k_none},
805 : {JSButton::button_function_t::k_mode_manual, JSButton::button_function_t::k_none},
806 : {JSButton::button_function_t::k_mode_depth_hold, JSButton::button_function_t::k_none},
807 : {JSButton::button_function_t::k_mode_stabilize, JSButton::button_function_t::k_none},
808 :
809 : {JSButton::button_function_t::k_disarm, JSButton::button_function_t::k_none},
810 : {JSButton::button_function_t::k_shift, JSButton::button_function_t::k_none},
811 : {JSButton::button_function_t::k_arm, JSButton::button_function_t::k_none},
812 : {JSButton::button_function_t::k_mount_center, JSButton::button_function_t::k_none},
813 :
814 : {JSButton::button_function_t::k_input_hold_set, JSButton::button_function_t::k_none},
815 : {JSButton::button_function_t::k_mount_tilt_down, JSButton::button_function_t::k_mount_pan_left},
816 : {JSButton::button_function_t::k_mount_tilt_up, JSButton::button_function_t::k_mount_pan_right},
817 : {JSButton::button_function_t::k_gain_inc, JSButton::button_function_t::k_trim_pitch_dec},
818 :
819 : {JSButton::button_function_t::k_gain_dec, JSButton::button_function_t::k_trim_pitch_inc},
820 : {JSButton::button_function_t::k_lights1_dimmer, JSButton::button_function_t::k_trim_roll_dec},
821 : {JSButton::button_function_t::k_lights1_brighter, JSButton::button_function_t::k_trim_roll_inc},
822 : {JSButton::button_function_t::k_none, JSButton::button_function_t::k_none},
823 : };
824 :
825 1411 : for (int i = 0; i < 16; i++) {
826 1328 : get_button(i)->set_default(defaults[i][0], defaults[i][1]);
827 : }
828 83 : }
829 :
830 109 : void Sub::set_neutral_controls()
831 : {
832 109 : channel_roll->set_radio_in(channel_roll->get_radio_trim());
833 109 : channel_pitch->set_radio_in(channel_pitch->get_radio_trim());
834 109 : channel_yaw->set_radio_in(channel_yaw->get_radio_trim());
835 109 : channel_throttle->set_radio_in(channel_throttle->get_radio_trim());
836 109 : channel_forward->set_radio_in(channel_forward->get_radio_trim());
837 109 : channel_lateral->set_radio_in(channel_lateral->get_radio_trim());
838 :
839 : // Clear pitch/roll trim settings
840 109 : pitchTrim = 0;
841 109 : rollTrim = 0;
842 109 : }
843 :
844 47 : void Sub::clear_input_hold()
845 : {
846 47 : xTrim = 0;
847 47 : yTrim = 0;
848 47 : zTrim = 0;
849 47 : input_hold_engaged = false;
850 47 : }
851 :
852 9457 : bool Sub::jsbutton_function_is_assigned(JSButton::button_function_t function)
853 : {
854 9457 : bool shift_is_assigned = false;
855 9457 : bool function_is_assigned_to_shift = false;
856 9457 : const uint8_t num_buttons = 32;
857 : // Check all 32 buttons for the specified function
858 180817 : for (uint8_t i = 0; i < num_buttons; i++) {
859 176048 : JSButton* current_button = get_button(i);
860 176048 : if (current_button == nullptr) {
861 0 : continue;
862 : }
863 176048 : if (current_button->function(false) == function) {
864 4688 : return true;
865 : }
866 171360 : if (current_button->function(false) == JSButton::k_shift) {
867 4769 : shift_is_assigned = true;
868 : }
869 171360 : if (current_button->function(true) == function) {
870 0 : if (shift_is_assigned) {
871 0 : return true;
872 : }
873 0 : function_is_assigned_to_shift = true;
874 : }
875 : }
876 4769 : return shift_is_assigned && function_is_assigned_to_shift;
877 : }
878 :
879 : #if AP_SCRIPTING_ENABLED
880 0 : bool Sub::is_button_pressed(uint8_t index)
881 : {
882 0 : return script_buttons[index - 1].is_pressed();
883 : }
884 :
885 0 : uint8_t Sub::get_and_clear_button_count(uint8_t index)
886 : {
887 0 : return script_buttons[index - 1].get_and_clear_count();
888 : }
889 : #endif // AP_SCRIPTING_ENABLED
|
