Line data Source code
1 : #include "Copter.h"
2 :
3 : #include "GCS_MAVLink_Copter.h"
4 : #include <AP_RPM/AP_RPM_config.h>
5 : #include <AP_EFI/AP_EFI_config.h>
6 :
7 86797 : MAV_TYPE GCS_Copter::frame_type() const
8 : {
9 : /*
10 : for GCS don't give MAV_TYPE_GENERIC as the GCS would have no
11 : information and won't display UIs such as flight mode
12 : selection
13 : */
14 : #if FRAME_CONFIG == HELI_FRAME
15 8575 : const MAV_TYPE mav_type_default = MAV_TYPE_HELICOPTER;
16 : #else
17 78222 : const MAV_TYPE mav_type_default = MAV_TYPE_QUADROTOR;
18 : #endif
19 86797 : if (copter.motors == nullptr) {
20 8 : return mav_type_default;
21 : }
22 86789 : MAV_TYPE mav_type = copter.motors->get_frame_mav_type();
23 86789 : if (mav_type == MAV_TYPE_GENERIC) {
24 518 : mav_type = mav_type_default;
25 : }
26 86789 : return mav_type;
27 : }
28 :
29 86797 : uint8_t GCS_MAVLINK_Copter::base_mode() const
30 : {
31 86797 : uint8_t _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
32 : // work out the base_mode. This value is not very useful
33 : // for APM, but we calculate it as best we can so a generic
34 : // MAVLink enabled ground station can work out something about
35 : // what the MAV is up to. The actual bit values are highly
36 : // ambiguous for most of the APM flight modes. In practice, you
37 : // only get useful information from the custom_mode, which maps to
38 : // the APM flight mode and has a well defined meaning in the
39 : // ArduPlane documentation
40 86797 : if ((copter.pos_control != nullptr) && copter.pos_control->NE_is_active()) {
41 50642 : _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
42 : // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
43 : // APM does in any mode, as that is defined as "system finds its own goal
44 : // positions", which APM does not currently do
45 : }
46 :
47 : // all modes except INITIALISING have some form of manual
48 : // override if stick mixing is enabled
49 86797 : _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
50 :
51 : // we are armed if we are not initialising
52 86797 : if (copter.motors != nullptr && copter.motors->armed()) {
53 51061 : _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
54 : }
55 :
56 : // indicate we have set a custom mode
57 86797 : _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
58 :
59 86797 : return _base_mode;
60 : }
61 :
62 86797 : uint32_t GCS_Copter::custom_mode() const
63 : {
64 86797 : return (uint32_t)copter.flightmode->mode_number();
65 : }
66 :
67 86797 : MAV_STATE GCS_MAVLINK_Copter::vehicle_system_status() const
68 : {
69 : // set system as critical if any failsafe have triggered
70 86797 : if (copter.any_failsafe_triggered()) {
71 2363 : return MAV_STATE_CRITICAL;
72 : }
73 :
74 84434 : if (copter.ap.land_complete) {
75 34874 : return MAV_STATE_STANDBY;
76 : }
77 :
78 49560 : if (!copter.ap.initialised) {
79 4247 : return MAV_STATE_BOOT;
80 : }
81 :
82 45313 : return MAV_STATE_ACTIVE;
83 : }
84 :
85 :
86 14690 : void GCS_MAVLINK_Copter::send_attitude_target()
87 : {
88 14690 : const Quaternion quat = copter.attitude_control->get_attitude_target_quat();
89 14690 : const Vector3f ang_vel = copter.attitude_control->get_attitude_target_ang_vel();
90 14690 : const float thrust = copter.attitude_control->get_throttle_in();
91 :
92 14690 : const float quat_out[4] {quat.q1, quat.q2, quat.q3, quat.q4};
93 :
94 : // Note: When sending out the attitude_target info. we send out all of info. no matter the mavlink typemask
95 : // This way we send out the maximum information that can be used by the sending control systems to adapt their generated trajectories
96 14690 : const uint16_t typemask = 0; // Ignore nothing
97 :
98 14690 : mavlink_msg_attitude_target_send(
99 : chan,
100 : AP_HAL::millis(), // time since boot (ms)
101 : typemask, // Bitmask that tells the system what control dimensions should be ignored by the vehicle
102 : quat_out, // Attitude quaternion [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], unit-length
103 14690 : ang_vel.x, // roll rate (rad/s)
104 14690 : ang_vel.y, // pitch rate (rad/s)
105 14690 : ang_vel.z, // yaw rate (rad/s)
106 : thrust); // Collective thrust, normalized to 0 .. 1
107 14690 : }
108 :
109 385956 : bool GCS_MAVLINK_Copter::get_target_location(Location &target) const
110 : {
111 385956 : return copter.flightmode->get_wp(target);
112 : }
113 :
114 4724 : void GCS_MAVLINK_Copter::send_position_target_local_ned()
115 : {
116 : #if MODE_GUIDED_ENABLED
117 4724 : if (!copter.flightmode->in_guided_mode()) {
118 2188 : return;
119 : }
120 :
121 2536 : const ModeGuided::SubMode guided_mode = copter.mode_guided.submode();
122 2536 : Vector3f target_pos_ned_m;
123 2536 : Vector3f target_vel_ned_ms;
124 2536 : Vector3f target_accel_ned_mss;
125 2536 : uint16_t type_mask = 0;
126 :
127 2536 : switch (guided_mode) {
128 0 : case ModeGuided::SubMode::Angle:
129 : // we don't have a local target when in angle mode
130 0 : return;
131 1117 : case ModeGuided::SubMode::TakeOff:
132 : case ModeGuided::SubMode::WP:
133 : case ModeGuided::SubMode::Pos:
134 1117 : type_mask = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE |
135 : POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
136 : POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position
137 1117 : target_pos_ned_m = copter.mode_guided.get_target_pos_NED_m().tofloat();
138 1117 : break;
139 784 : case ModeGuided::SubMode::PosVelAccel:
140 784 : type_mask = POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position, velocity & acceleration
141 784 : target_pos_ned_m = copter.mode_guided.get_target_pos_NED_m().tofloat();
142 784 : target_vel_ned_ms = copter.mode_guided.get_target_vel_NED_ms();
143 784 : target_accel_ned_mss = copter.mode_guided.get_target_accel_NED_mss();
144 784 : break;
145 568 : case ModeGuided::SubMode::VelAccel:
146 568 : type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE |
147 : POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity & acceleration
148 568 : target_vel_ned_ms = copter.mode_guided.get_target_vel_NED_ms();
149 568 : target_accel_ned_mss = copter.mode_guided.get_target_accel_NED_mss();
150 568 : break;
151 67 : case ModeGuided::SubMode::Accel:
152 67 : type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE |
153 : POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE |
154 : POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity & acceleration
155 67 : target_accel_ned_mss = copter.mode_guided.get_target_accel_NED_mss();
156 67 : break;
157 : }
158 :
159 2536 : mavlink_msg_position_target_local_ned_send(
160 : chan,
161 : AP_HAL::millis(), // time boot ms
162 : MAV_FRAME_LOCAL_NED,
163 : type_mask,
164 : target_pos_ned_m.x, // x in metres
165 : target_pos_ned_m.y, // y in metres
166 : target_pos_ned_m.z, // z in metres NED frame
167 : target_vel_ned_ms.x, // vx in m/s
168 : target_vel_ned_ms.y, // vy in m/s
169 : target_vel_ned_ms.z, // vz in m/s NED frame
170 : target_accel_ned_mss.x, // afx in m/s/s
171 : target_accel_ned_mss.y, // afy in m/s/s
172 : target_accel_ned_mss.z, // afz in m/s/s NED frame
173 : 0.0f, // yaw
174 : 0.0f); // yaw_rate
175 : #endif
176 : }
177 :
178 385952 : void GCS_MAVLINK_Copter::send_nav_controller_output() const
179 : {
180 385952 : if (!copter.ap.initialised) {
181 70 : return;
182 : }
183 385882 : const Vector3f &targets_rad = copter.attitude_control->get_att_target_euler_rad();
184 385882 : const Mode *flightmode = copter.flightmode;
185 2701174 : mavlink_msg_nav_controller_output_send(
186 385882 : chan,
187 385882 : degrees(targets_rad.x),
188 385882 : degrees(targets_rad.y),
189 385882 : degrees(targets_rad.z),
190 385882 : flightmode->wp_bearing_deg(),
191 771764 : MIN(flightmode->wp_distance_m(), UINT16_MAX),
192 385882 : copter.pos_control->get_pos_error_D_m(),
193 : 0,
194 385882 : flightmode->crosstrack_error_m());
195 : }
196 :
197 385992 : float GCS_MAVLINK_Copter::vfr_hud_airspeed() const
198 : {
199 : #if AP_AIRSPEED_ENABLED
200 : // airspeed sensors are best. While the AHRS airspeed_estimate
201 : // will use an airspeed sensor, that value is constrained by the
202 : // ground speed. When reporting we should send the true airspeed
203 : // value if possible:
204 385992 : if (copter.airspeed.enabled() && copter.airspeed.healthy()) {
205 3622 : return copter.airspeed.get_airspeed();
206 : }
207 : #endif
208 :
209 382370 : Vector3f airspeed_vec_bf;
210 382370 : if (AP::ahrs().airspeed_vector_TAS(airspeed_vec_bf)) {
211 : // we are running the EKF3 wind estimation code which can give
212 : // us an airspeed estimate
213 256906 : return airspeed_vec_bf.length();
214 : }
215 125464 : return AP::gps().ground_speed();
216 : }
217 :
218 386540 : int16_t GCS_MAVLINK_Copter::vfr_hud_throttle() const
219 : {
220 386540 : if (copter.motors == nullptr) {
221 0 : return 0;
222 : }
223 386540 : return (int16_t)(copter.motors->get_throttle() * 100);
224 : }
225 :
226 : /*
227 : send PID tuning message
228 : */
229 385952 : void GCS_MAVLINK_Copter::send_pid_tuning()
230 : {
231 : static const PID_TUNING_AXIS axes[] = {
232 : PID_TUNING_ROLL,
233 : PID_TUNING_PITCH,
234 : PID_TUNING_YAW,
235 : PID_TUNING_ACCZ
236 : };
237 1929760 : for (uint8_t i=0; i<ARRAY_SIZE(axes); i++) {
238 1543808 : if (!(copter.g.gcs_pid_mask & (1<<(axes[i]-1)))) {
239 1540602 : continue;
240 : }
241 3206 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
242 0 : return;
243 : }
244 3206 : const AP_PIDInfo *pid_info = nullptr;
245 3206 : switch (axes[i]) {
246 1074 : case PID_TUNING_ROLL:
247 1074 : pid_info = &copter.attitude_control->get_rate_roll_pid().get_pid_info();
248 1074 : break;
249 1066 : case PID_TUNING_PITCH:
250 1066 : pid_info = &copter.attitude_control->get_rate_pitch_pid().get_pid_info();
251 1066 : break;
252 1066 : case PID_TUNING_YAW:
253 1066 : pid_info = &copter.attitude_control->get_rate_yaw_pid().get_pid_info();
254 1066 : break;
255 0 : case PID_TUNING_ACCZ:
256 0 : pid_info = &copter.pos_control->D_get_accel_pid().get_pid_info();
257 0 : break;
258 0 : default:
259 0 : continue;
260 : }
261 3206 : if (pid_info != nullptr) {
262 3206 : mavlink_msg_pid_tuning_send(chan,
263 3206 : axes[i],
264 3206 : pid_info->target,
265 3206 : pid_info->actual,
266 3206 : pid_info->FF,
267 3206 : pid_info->P,
268 3206 : pid_info->I,
269 3206 : pid_info->D,
270 3206 : pid_info->slew_rate,
271 3206 : pid_info->Dmod);
272 : }
273 : }
274 : }
275 :
276 : #if AP_WINCH_ENABLED
277 : // send winch status message
278 385952 : void GCS_MAVLINK_Copter::send_winch_status() const
279 : {
280 385952 : AP_Winch *winch = AP::winch();
281 385952 : if (winch == nullptr) {
282 0 : return;
283 : }
284 385952 : winch->send_status(*this);
285 : }
286 : #endif
287 :
288 999161 : bool GCS_Copter::vehicle_initialised() const {
289 999161 : return copter.ap.initialised;
290 : }
291 :
292 : // try to send a message, return false if it wasn't sent
293 79956949 : bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
294 : {
295 79956949 : switch(id) {
296 :
297 386167 : case MSG_WIND:
298 386167 : CHECK_PAYLOAD_SIZE(WIND);
299 386167 : send_wind();
300 386167 : break;
301 :
302 385952 : case MSG_ADSB_VEHICLE: {
303 : #if HAL_ADSB_ENABLED
304 385952 : CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
305 385952 : copter.adsb.send_adsb_vehicle(chan);
306 : #endif
307 : #if AP_OAPATHPLANNER_ENABLED
308 385952 : AP_OADatabase *oadb = AP_OADatabase::get_singleton();
309 385952 : if (oadb != nullptr) {
310 385952 : CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
311 385952 : uint16_t interval_ms = 0;
312 385952 : if (get_ap_message_interval(id, interval_ms)) {
313 385952 : oadb->send_adsb_vehicle(chan, interval_ms);
314 : }
315 : }
316 : #endif
317 385952 : break;
318 : }
319 :
320 79184830 : default:
321 79184830 : return GCS_MAVLINK::try_send_message(id);
322 : }
323 772119 : return true;
324 : }
325 :
326 :
327 : #if MODE_AUTO_ENABLED
328 386004 : MISSION_STATE GCS_MAVLINK_Copter::mission_state(const class AP_Mission &mission) const
329 : {
330 386004 : if (copter.mode_auto.paused()) {
331 135 : return MISSION_STATE_PAUSED;
332 : }
333 385869 : return GCS_MAVLINK::mission_state(mission);
334 : }
335 : #endif // MODE_AUTO_ENABLED
336 :
337 0 : bool GCS_MAVLINK_Copter::handle_guided_request(AP_Mission::Mission_Command &cmd)
338 : {
339 : #if MODE_AUTO_ENABLED
340 0 : return copter.mode_auto.do_guided(cmd);
341 : #else
342 : return false;
343 : #endif
344 : }
345 :
346 1210992 : void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status,
347 : const mavlink_message_t &msg)
348 : {
349 : // we handle these messages here to avoid them being blocked by mavlink routing code
350 : #if AP_ADSB_AVOIDANCE_ENABLED
351 1210992 : if (copter.g2.dev_options.get() & DevOptionADSBMAVLink) {
352 : // optional handling of GLOBAL_POSITION_INT as a MAVLink based avoidance source
353 0 : copter.avoidance_adsb.handle_msg(msg);
354 : }
355 : #endif
356 1210992 : GCS_MAVLINK::packetReceived(status, msg);
357 1210992 : }
358 :
359 436 : bool GCS_MAVLINK_Copter::params_ready() const
360 : {
361 436 : if (AP_BoardConfig::in_config_error()) {
362 : // we may never have parameters "initialised" in this case
363 0 : return true;
364 : }
365 : // if we have not yet initialised (including allocating the motors
366 : // object) we drop this request. That prevents the GCS from getting
367 : // a confusing parameter count during bootup
368 436 : return copter.ap.initialised_params;
369 : }
370 :
371 436 : void GCS_MAVLINK_Copter::send_banner()
372 : {
373 436 : GCS_MAVLINK::send_banner();
374 436 : if (copter.motors == nullptr) {
375 0 : send_text(MAV_SEVERITY_INFO, "motors not allocated");
376 0 : return;
377 : }
378 : char frame_and_type_string[30];
379 436 : copter.motors->get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string));
380 436 : send_text(MAV_SEVERITY_INFO, "%s", frame_and_type_string);
381 : }
382 :
383 7448 : void GCS_MAVLINK_Copter::handle_command_ack(const mavlink_message_t &msg)
384 : {
385 7448 : copter.command_ack_counter++;
386 7448 : GCS_MAVLINK::handle_command_ack(msg);
387 7448 : }
388 :
389 : /*
390 : handle a LANDING_TARGET command. The timestamp has been jitter corrected
391 : */
392 671 : void GCS_MAVLINK_Copter::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
393 : {
394 : #if AC_PRECLAND_ENABLED
395 671 : copter.precland.handle_msg(packet, timestamp_ms);
396 : #endif
397 671 : }
398 :
399 12 : MAV_RESULT GCS_MAVLINK_Copter::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
400 : {
401 12 : if (packet.y == 1) {
402 : // compassmot calibration
403 1 : return copter.mavlink_compassmot(*this);
404 : }
405 :
406 11 : return GCS_MAVLINK::_handle_command_preflight_calibration(packet, msg);
407 : }
408 :
409 :
410 52 : MAV_RESULT GCS_MAVLINK_Copter::handle_command_do_set_roi(const Location &roi_loc)
411 : {
412 52 : if (!roi_loc.check_latlng()) {
413 0 : return MAV_RESULT_FAILED;
414 : }
415 52 : copter.flightmode->auto_yaw.set_roi(roi_loc);
416 52 : return MAV_RESULT_ACCEPTED;
417 : }
418 :
419 806 : MAV_RESULT GCS_MAVLINK_Copter::handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
420 : {
421 : // reject reboot if user has also specified they want the "Auto" ESC calibration on next reboot
422 806 : if (copter.g.esc_calibrate == (uint8_t)Copter::ESCCalibrationModes::ESCCAL_AUTO) {
423 0 : send_text(MAV_SEVERITY_CRITICAL, "Reboot rejected, ESC cal on reboot");
424 0 : return MAV_RESULT_FAILED;
425 : }
426 :
427 : // call parent
428 806 : return GCS_MAVLINK::handle_preflight_reboot(packet, msg);
429 : }
430 :
431 7 : MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
432 : {
433 : #if MODE_GUIDED_ENABLED
434 7 : const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
435 7 : if (!copter.flightmode->in_guided_mode() && !change_modes) {
436 0 : return MAV_RESULT_DENIED;
437 : }
438 :
439 7 : Location request_location;
440 7 : if (!location_from_command_t(packet, request_location)) {
441 0 : return MAV_RESULT_DENIED;
442 : }
443 :
444 7 : if (request_location.sanitize(copter.current_loc)) {
445 : // if the location wasn't already sane don't load it
446 0 : return MAV_RESULT_DENIED; // failed as the location is not valid
447 : }
448 :
449 : // we need to do this first, as we don't want to change the flight mode unless we can also set the target
450 7 : if (!copter.mode_guided.set_destination(request_location, false, 0, false, 0)) {
451 1 : return MAV_RESULT_FAILED;
452 : }
453 :
454 6 : if (!copter.flightmode->in_guided_mode()) {
455 2 : if (!copter.set_mode(Mode::Number::GUIDED, ModeReason::GCS_COMMAND)) {
456 0 : return MAV_RESULT_FAILED;
457 : }
458 : // the position won't have been loaded if we had to change the flight mode, so load it again
459 2 : if (!copter.mode_guided.set_destination(request_location, false, 0, false, 0)) {
460 0 : return MAV_RESULT_FAILED;
461 : }
462 : }
463 :
464 6 : return MAV_RESULT_ACCEPTED;
465 : #else
466 : return MAV_RESULT_UNSUPPORTED;
467 : #endif
468 : }
469 :
470 256497 : MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
471 : {
472 256497 : switch(packet.command) {
473 :
474 124 : case MAV_CMD_CONDITION_YAW:
475 124 : return handle_MAV_CMD_CONDITION_YAW(packet);
476 :
477 6 : case MAV_CMD_DO_CHANGE_SPEED:
478 6 : return handle_MAV_CMD_DO_CHANGE_SPEED(packet);
479 :
480 7 : case MAV_CMD_DO_REPOSITION:
481 7 : return handle_command_int_do_reposition(packet);
482 :
483 : // pause or resume an auto mission
484 18 : case MAV_CMD_DO_PAUSE_CONTINUE:
485 18 : return handle_command_pause_continue(packet);
486 :
487 4 : case MAV_CMD_DO_MOTOR_TEST:
488 4 : return handle_MAV_CMD_DO_MOTOR_TEST(packet);
489 :
490 147 : case MAV_CMD_NAV_TAKEOFF:
491 : case MAV_CMD_NAV_VTOL_TAKEOFF:
492 147 : return handle_MAV_CMD_NAV_TAKEOFF(packet);
493 :
494 : #if HAL_PARACHUTE_ENABLED
495 12 : case MAV_CMD_DO_PARACHUTE:
496 12 : return handle_MAV_CMD_DO_PARACHUTE(packet);
497 : #endif
498 :
499 : #if AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED
500 : // Solo user presses pause button
501 0 : case MAV_CMD_SOLO_BTN_PAUSE_CLICK:
502 0 : return handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(packet);
503 : // Solo user presses Fly button:
504 0 : case MAV_CMD_SOLO_BTN_FLY_HOLD:
505 0 : return handle_MAV_CMD_SOLO_BTN_FLY_HOLD(packet);
506 : // Solo user holds down Fly button for a couple of seconds
507 0 : case MAV_CMD_SOLO_BTN_FLY_CLICK:
508 0 : return handle_MAV_CMD_SOLO_BTN_FLY_CLICK(packet);
509 : #endif
510 :
511 : #if MODE_AUTO_ENABLED
512 5 : case MAV_CMD_MISSION_START:
513 5 : return handle_MAV_CMD_MISSION_START(packet);
514 : #endif
515 :
516 : #if AP_WINCH_ENABLED
517 10 : case MAV_CMD_DO_WINCH:
518 10 : return handle_MAV_CMD_DO_WINCH(packet);
519 : #endif
520 :
521 2 : case MAV_CMD_NAV_LOITER_UNLIM:
522 2 : if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
523 0 : return MAV_RESULT_FAILED;
524 : }
525 2 : return MAV_RESULT_ACCEPTED;
526 :
527 2 : case MAV_CMD_NAV_RETURN_TO_LAUNCH:
528 2 : if (!copter.set_mode(Mode::Number::RTL, ModeReason::GCS_COMMAND)) {
529 0 : return MAV_RESULT_FAILED;
530 : }
531 2 : return MAV_RESULT_ACCEPTED;
532 :
533 4 : case MAV_CMD_NAV_VTOL_LAND:
534 : case MAV_CMD_NAV_LAND:
535 4 : if (!copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND)) {
536 0 : return MAV_RESULT_FAILED;
537 : }
538 4 : return MAV_RESULT_ACCEPTED;
539 :
540 : #if MODE_AUTO_ENABLED
541 5 : case MAV_CMD_DO_RETURN_PATH_START:
542 5 : if (copter.mode_auto.return_path_start_auto_RTL(ModeReason::GCS_COMMAND)) {
543 3 : return MAV_RESULT_ACCEPTED;
544 : }
545 2 : return MAV_RESULT_FAILED;
546 :
547 5 : case MAV_CMD_DO_LAND_START:
548 5 : if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) {
549 4 : return MAV_RESULT_ACCEPTED;
550 : }
551 1 : return MAV_RESULT_FAILED;
552 : #endif
553 :
554 256146 : default:
555 256146 : return GCS_MAVLINK::handle_command_int_packet(packet, msg);
556 : }
557 : }
558 :
559 : #if HAL_MOUNT_ENABLED
560 437 : MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
561 : {
562 437 : switch (packet.command) {
563 36 : case MAV_CMD_DO_MOUNT_CONTROL:
564 : // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
565 8 : if (((MAV_MOUNT_MODE)packet.z == MAV_MOUNT_MODE_MAVLINK_TARGETING) &&
566 44 : (copter.camera_mount.get_mount_type() != AP_Mount::Type::None) &&
567 8 : !copter.camera_mount.has_pan_control()) {
568 : // Per the handler in AP_Mount, DO_MOUNT_CONTROL yaw angle is in body frame, which is
569 : // equivalent to an offset to the current yaw demand.
570 3 : copter.flightmode->auto_yaw.set_yaw_angle_offset_deg(packet.param3);
571 : }
572 36 : break;
573 401 : default:
574 401 : break;
575 : }
576 437 : return GCS_MAVLINK::handle_command_mount(packet, msg);
577 : }
578 : #endif
579 :
580 147 : MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_NAV_TAKEOFF(const mavlink_command_int_t &packet)
581 : {
582 147 : if (packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) {
583 0 : return MAV_RESULT_DENIED; // meaning some parameters are bad
584 : }
585 :
586 : // param3 : horizontal navigation by pilot acceptable
587 : // param4 : yaw angle (not supported)
588 : // param5 : latitude (not supported)
589 : // param6 : longitude (not supported)
590 : // param7 : altitude [metres]
591 :
592 147 : float takeoff_alt_m = packet.z;
593 :
594 147 : const bool must_navigate = ((uint32_t(packet.param3) & NAV_TAKEOFF_FLAGS_HORIZONTAL_POSITION_NOT_REQUIRED) == 0);
595 147 : if (!copter.flightmode->do_user_takeoff_U_m(takeoff_alt_m, must_navigate)) {
596 2 : return MAV_RESULT_FAILED;
597 : }
598 145 : return MAV_RESULT_ACCEPTED;
599 : }
600 :
601 : #if AP_MAVLINK_COMMAND_LONG_ENABLED
602 27 : bool GCS_MAVLINK_Copter::mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD packet_command) const
603 : {
604 27 : if (packet_command == MAV_CMD_NAV_TAKEOFF ||
605 : packet_command == MAV_CMD_NAV_VTOL_TAKEOFF) {
606 0 : frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
607 0 : return true;
608 : }
609 27 : return GCS_MAVLINK::mav_frame_for_command_long(frame, packet_command);
610 : }
611 : #endif
612 :
613 :
614 124 : MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet)
615 : {
616 : // param1 : target angle [0-360]
617 : // param2 : speed during change [deg per second]
618 : // param3 : direction (-1:ccw, +1:cw)
619 : // param4 : relative offset (1) or absolute angle (0)
620 372 : if ((packet.param1 >= 0.0f) &&
621 338 : (packet.param1 <= 360.0f) &&
622 214 : (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) {
623 124 : copter.flightmode->auto_yaw.set_fixed_yaw_rad(
624 124 : radians(packet.param1),
625 124 : radians(packet.param2),
626 124 : (int8_t)packet.param3,
627 124 : is_positive(packet.param4));
628 124 : return MAV_RESULT_ACCEPTED;
629 : }
630 0 : return MAV_RESULT_FAILED;
631 : }
632 :
633 6 : MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet)
634 : {
635 6 : if (!is_positive(packet.param2)) {
636 : // Target speed must be larger than zero
637 0 : return MAV_RESULT_DENIED;
638 : }
639 :
640 6 : const float speed_ms = packet.param2;
641 :
642 6 : bool success = false;
643 6 : switch (SPEED_TYPE(packet.param1)) {
644 0 : case SPEED_TYPE_ENUM_END:
645 0 : return MAV_RESULT_DENIED;
646 :
647 6 : case SPEED_TYPE_AIRSPEED: // Airspeed is treated as ground speed for GCS compatibility
648 : case SPEED_TYPE_GROUNDSPEED:
649 6 : success = copter.flightmode->set_speed_NE_ms(speed_ms);
650 6 : break;
651 :
652 0 : case SPEED_TYPE_CLIMB_SPEED:
653 0 : success = copter.flightmode->set_speed_up_ms(speed_ms);
654 0 : break;
655 :
656 0 : case SPEED_TYPE_DESCENT_SPEED:
657 0 : success = copter.flightmode->set_speed_down_ms(speed_ms);
658 0 : break;
659 : }
660 :
661 6 : return success ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
662 : }
663 :
664 : #if MODE_AUTO_ENABLED
665 5 : MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet)
666 : {
667 5 : if (!is_zero(packet.param1) || !is_zero(packet.param2)) {
668 : // first-item/last item not supported
669 3 : return MAV_RESULT_DENIED;
670 : }
671 2 : if (copter.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) {
672 2 : copter.set_auto_armed(true);
673 2 : if (copter.mode_auto.mission.state() != AP_Mission::MISSION_RUNNING) {
674 0 : copter.mode_auto.mission.start_or_resume();
675 : }
676 2 : return MAV_RESULT_ACCEPTED;
677 : }
678 0 : return MAV_RESULT_FAILED;
679 : }
680 : #endif
681 :
682 :
683 :
684 : #if HAL_PARACHUTE_ENABLED
685 12 : MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet)
686 : {
687 : // configure or release parachute
688 12 : switch ((uint16_t)packet.param1) {
689 2 : case PARACHUTE_DISABLE:
690 2 : copter.parachute.enabled(false);
691 2 : return MAV_RESULT_ACCEPTED;
692 4 : case PARACHUTE_ENABLE:
693 4 : copter.parachute.enabled(true);
694 4 : return MAV_RESULT_ACCEPTED;
695 6 : case PARACHUTE_RELEASE:
696 : // treat as a manual release which performs some additional check of altitude
697 6 : copter.parachute_manual_release();
698 6 : return MAV_RESULT_ACCEPTED;
699 : }
700 0 : return MAV_RESULT_FAILED;
701 : }
702 : #endif
703 :
704 4 : MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet)
705 : {
706 : // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
707 : // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
708 : // param3 : throttle (range depends upon param2)
709 : // param4 : timeout (in seconds)
710 : // param5 : num_motors (in sequence)
711 : // param6 : motor test order
712 4 : return copter.mavlink_motor_test_start(*this,
713 4 : (uint8_t)packet.param1,
714 4 : (uint8_t)packet.param2,
715 4 : packet.param3,
716 4 : packet.param4,
717 4 : (uint8_t)packet.x);
718 : }
719 :
720 : #if AP_WINCH_ENABLED
721 10 : MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_DO_WINCH(const mavlink_command_int_t &packet)
722 : {
723 : // param1 : winch number (ignored)
724 : // param2 : action (0=relax, 1=relative length control, 2=rate control). See WINCH_ACTIONS enum.
725 10 : if (!copter.g2.winch.enabled()) {
726 0 : return MAV_RESULT_FAILED;
727 : }
728 10 : switch ((uint8_t)packet.param2) {
729 4 : case WINCH_RELAXED:
730 4 : copter.g2.winch.relax();
731 4 : return MAV_RESULT_ACCEPTED;
732 2 : case WINCH_RELATIVE_LENGTH_CONTROL: {
733 2 : copter.g2.winch.release_length(packet.param3);
734 2 : return MAV_RESULT_ACCEPTED;
735 : }
736 4 : case WINCH_RATE_CONTROL:
737 4 : copter.g2.winch.set_desired_rate(packet.param4);
738 4 : return MAV_RESULT_ACCEPTED;
739 0 : default:
740 0 : break;
741 : }
742 0 : return MAV_RESULT_FAILED;
743 : }
744 : #endif // AP_WINCH_ENABLED
745 :
746 : #if AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED
747 0 : MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_FLY_CLICK(const mavlink_command_int_t &packet)
748 : {
749 0 : if (copter.failsafe.radio) {
750 0 : return MAV_RESULT_ACCEPTED;
751 : }
752 :
753 : // set mode to Loiter or fall back to AltHold
754 0 : if (!copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
755 0 : copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND);
756 : }
757 0 : return MAV_RESULT_ACCEPTED;
758 : }
759 :
760 0 : MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_FLY_HOLD(const mavlink_command_int_t &packet)
761 : {
762 0 : if (copter.failsafe.radio) {
763 0 : return MAV_RESULT_ACCEPTED;
764 : }
765 :
766 0 : if (!copter.motors->armed()) {
767 : // if disarmed, arm motors
768 0 : copter.arming.arm(AP_Arming::Method::MAVLINK);
769 0 : } else if (copter.ap.land_complete) {
770 : // if armed and landed, takeoff
771 0 : if (copter.set_mode(Mode::Number::LOITER, ModeReason::GCS_COMMAND)) {
772 0 : copter.flightmode->do_user_takeoff_U_m(packet.param1, true);
773 : }
774 : } else {
775 : // if flying, land
776 0 : copter.set_mode(Mode::Number::LAND, ModeReason::GCS_COMMAND);
777 : }
778 0 : return MAV_RESULT_ACCEPTED;
779 : }
780 :
781 0 : MAV_RESULT GCS_MAVLINK_Copter::handle_MAV_CMD_SOLO_BTN_PAUSE_CLICK(const mavlink_command_int_t &packet)
782 : {
783 0 : if (copter.failsafe.radio) {
784 0 : return MAV_RESULT_ACCEPTED;
785 : }
786 :
787 0 : if (copter.motors->armed()) {
788 0 : if (copter.ap.land_complete) {
789 : // if landed, disarm motors
790 0 : copter.arming.disarm(AP_Arming::Method::SOLOPAUSEWHENLANDED);
791 : } else {
792 : // assume that shots modes are all done in guided.
793 : // NOTE: this may need to change if we add a non-guided shot mode
794 0 : bool shot_mode = (!is_zero(packet.param1) && (copter.flightmode->mode_number() == Mode::Number::GUIDED || copter.flightmode->mode_number() == Mode::Number::GUIDED_NOGPS));
795 :
796 0 : if (!shot_mode) {
797 : #if MODE_BRAKE_ENABLED
798 0 : if (copter.set_mode(Mode::Number::BRAKE, ModeReason::GCS_COMMAND)) {
799 0 : copter.mode_brake.timeout_to_loiter_ms(2500);
800 : } else {
801 0 : copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND);
802 : }
803 : #else
804 : copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::GCS_COMMAND);
805 : #endif
806 : } else {
807 : // SoloLink is expected to handle pause in shots
808 : }
809 : }
810 : }
811 0 : return MAV_RESULT_ACCEPTED;
812 : }
813 : #endif // AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED
814 :
815 18 : MAV_RESULT GCS_MAVLINK_Copter::handle_command_pause_continue(const mavlink_command_int_t &packet)
816 : {
817 : // requested pause
818 18 : if ((uint8_t) packet.param1 == 0) {
819 9 : if (copter.flightmode->pause()) {
820 9 : return MAV_RESULT_ACCEPTED;
821 : }
822 0 : send_text(MAV_SEVERITY_INFO, "Failed to pause");
823 0 : return MAV_RESULT_FAILED;
824 : }
825 :
826 : // requested resume
827 9 : if ((uint8_t) packet.param1 == 1) {
828 9 : if (copter.flightmode->resume()) {
829 9 : return MAV_RESULT_ACCEPTED;
830 : }
831 0 : send_text(MAV_SEVERITY_INFO, "Failed to resume");
832 0 : return MAV_RESULT_FAILED;
833 : }
834 0 : return MAV_RESULT_DENIED;
835 : }
836 :
837 : // this is called on receipt of a MANUAL_CONTROL packet and is
838 : // expected to call manual_override to override RC input on desired
839 : // axes.
840 3 : void GCS_MAVLINK_Copter::handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow)
841 : {
842 3 : if (packet.z < 0) { // Copter doesn't do negative thrust
843 0 : return;
844 : }
845 :
846 3 : manual_override(copter.channel_roll, packet.y, 1000, 2000, tnow);
847 3 : manual_override(copter.channel_pitch, packet.x, 1000, 2000, tnow, true);
848 3 : manual_override(copter.channel_throttle, packet.z, 0, 1000, tnow);
849 3 : manual_override(copter.channel_yaw, packet.r, 1000, 2000, tnow);
850 : }
851 :
852 : // sanity check velocity or acceleration vector components are numbers
853 : // (e.g. not NaN) and below 1000. vec argument units are in meters/second or
854 : // metres/second/second
855 2919 : bool GCS_MAVLINK_Copter::sane_vel_or_acc_vector(const Vector3f &vec) const
856 : {
857 11361 : for (uint8_t i=0; i<3; i++) {
858 : // consider velocity invalid if any component nan or >1000(m/s or m/s/s)
859 8547 : if (isnan(vec[i]) || fabsf(vec[i]) > 1000) {
860 105 : return false;
861 : }
862 : }
863 2814 : return true;
864 : }
865 :
866 : #if MODE_GUIDED_ENABLED
867 : // for mavlink SET_POSITION_TARGET messages
868 : constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE =
869 : POSITION_TARGET_TYPEMASK_X_IGNORE |
870 : POSITION_TARGET_TYPEMASK_Y_IGNORE |
871 : POSITION_TARGET_TYPEMASK_Z_IGNORE;
872 :
873 : constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE =
874 : POSITION_TARGET_TYPEMASK_VX_IGNORE |
875 : POSITION_TARGET_TYPEMASK_VY_IGNORE |
876 : POSITION_TARGET_TYPEMASK_VZ_IGNORE;
877 :
878 : constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE =
879 : POSITION_TARGET_TYPEMASK_AX_IGNORE |
880 : POSITION_TARGET_TYPEMASK_AY_IGNORE |
881 : POSITION_TARGET_TYPEMASK_AZ_IGNORE;
882 :
883 : constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE =
884 : POSITION_TARGET_TYPEMASK_YAW_IGNORE;
885 : constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE =
886 : POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE;
887 : constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_FORCE_SET =
888 : POSITION_TARGET_TYPEMASK_FORCE_SET;
889 : #endif
890 :
891 : #if MODE_GUIDED_ENABLED
892 115 : void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_message_t &msg)
893 : {
894 : // decode packet
895 : mavlink_set_attitude_target_t packet;
896 115 : mavlink_msg_set_attitude_target_decode(&msg, &packet);
897 :
898 : // exit if vehicle is not in Guided mode or Auto-Guided mode
899 115 : if (!copter.flightmode->in_guided_mode()) {
900 54 : return;
901 : }
902 :
903 61 : const bool roll_rate_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE;
904 61 : const bool pitch_rate_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE;
905 61 : const bool yaw_rate_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE;
906 61 : const bool throttle_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE;
907 61 : const bool attitude_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE;
908 :
909 : // ensure thrust field is not ignored
910 61 : if (throttle_ignore) {
911 : // The throttle input is not defined
912 0 : copter.mode_guided.hold_position();
913 0 : return;
914 : }
915 :
916 61 : Quaternion attitude_quat;
917 61 : if (attitude_ignore) {
918 0 : attitude_quat.zero();
919 : } else {
920 61 : attitude_quat = Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]);
921 :
922 : // Do not accept the attitude_quaternion
923 : // if its magnitude is not close to unit length +/- 1E-3
924 : // this limit is somewhat greater than sqrt(FLT_EPSL)
925 61 : if (!attitude_quat.is_unit_length()) {
926 : // The attitude quaternion is ill-defined
927 0 : copter.mode_guided.hold_position();
928 0 : return;
929 : }
930 : }
931 :
932 61 : Vector3f ang_vel_body;
933 61 : if (!roll_rate_ignore && !pitch_rate_ignore && !yaw_rate_ignore) {
934 61 : ang_vel_body.x = packet.body_roll_rate;
935 61 : ang_vel_body.y = packet.body_pitch_rate;
936 61 : ang_vel_body.z = packet.body_yaw_rate;
937 0 : } else if (!(roll_rate_ignore && pitch_rate_ignore && yaw_rate_ignore)) {
938 : // The body rates are ill-defined
939 0 : copter.mode_guided.hold_position();
940 0 : return;
941 : }
942 :
943 : // check if the message's thrust field should be interpreted as a climb rate or as thrust
944 61 : const bool use_thrust = copter.mode_guided.set_attitude_target_provides_thrust();
945 :
946 : float climb_rate_ms_or_thrust;
947 61 : if (use_thrust) {
948 : // interpret thrust as thrust
949 1 : climb_rate_ms_or_thrust = constrain_float(packet.thrust, -1.0f, 1.0f);
950 : } else {
951 : // convert thrust to climb rate
952 60 : packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f);
953 60 : if (is_equal(packet.thrust, 0.5f)) {
954 16 : climb_rate_ms_or_thrust = 0.0f;
955 44 : } else if (packet.thrust > 0.5f) {
956 : // climb at up to WP_SPD_UP
957 44 : climb_rate_ms_or_thrust = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_default_speed_up_ms();
958 : } else {
959 : // descend at up to WP_SPD_DN
960 0 : climb_rate_ms_or_thrust = (0.5f - packet.thrust) * 2.0f * -copter.wp_nav->get_default_speed_down_ms();
961 : }
962 : }
963 :
964 61 : copter.mode_guided.set_angle(attitude_quat, ang_vel_body,
965 : climb_rate_ms_or_thrust, use_thrust);
966 : }
967 :
968 544 : void GCS_MAVLINK_Copter::handle_message_set_position_target_local_ned(const mavlink_message_t &msg)
969 : {
970 : // decode packet
971 : mavlink_set_position_target_local_ned_t packet;
972 544 : mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
973 :
974 : // exit if vehicle is not in Guided mode or Auto-Guided mode
975 544 : if (!copter.flightmode->in_guided_mode()) {
976 51 : return;
977 : }
978 :
979 : // check for supported coordinate frames
980 544 : if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
981 1 : packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
982 1 : packet.coordinate_frame != MAV_FRAME_BODY_NED &&
983 0 : packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
984 : // unsupported coordinate frame
985 0 : copter.mode_guided.hold_position();
986 0 : return;
987 : }
988 :
989 544 : bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
990 544 : bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
991 544 : bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
992 544 : bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
993 544 : bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
994 544 : bool force_set = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE_SET;
995 :
996 : // Force inputs are not supported
997 : // Do not accept command if force_set is true and acc_ignore is false
998 544 : if (force_set && !acc_ignore) {
999 0 : copter.mode_guided.hold_position();
1000 0 : return;
1001 : }
1002 :
1003 : // prepare position
1004 544 : Vector3p pos_ned_m;
1005 544 : if (!pos_ignore) {
1006 : // convert to m
1007 54 : pos_ned_m = Vector3p{packet.x, packet.y, packet.z};
1008 : // rotate to earth-frame if necessary
1009 54 : if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
1010 53 : packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
1011 1 : pos_ned_m.xy() = copter.ahrs.body_to_earth2D_p(pos_ned_m.xy());
1012 : }
1013 : // add body offset if necessary
1014 54 : if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
1015 54 : packet.coordinate_frame == MAV_FRAME_BODY_NED ||
1016 53 : packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
1017 1 : Vector3p rel_pos_ned_m;
1018 1 : if (!AP::ahrs().get_relative_position_NED_origin(rel_pos_ned_m)) {
1019 : // need position estimate to calculate target position
1020 0 : copter.mode_guided.hold_position();
1021 0 : return;
1022 : }
1023 1 : pos_ned_m += rel_pos_ned_m;
1024 : }
1025 : }
1026 :
1027 : // prepare velocity
1028 544 : Vector3f vel_ned_ms;
1029 544 : if (!vel_ignore) {
1030 490 : vel_ned_ms = Vector3f{packet.vx, packet.vy, packet.vz};
1031 490 : if (!sane_vel_or_acc_vector(vel_ned_ms)) {
1032 : // velocity vector contains NaN or Inf
1033 51 : copter.mode_guided.hold_position();
1034 51 : return;
1035 : }
1036 : // rotate to earth-frame if necessary
1037 439 : if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
1038 0 : vel_ned_ms.xy() = copter.ahrs.body_to_earth2D(vel_ned_ms.xy());
1039 : }
1040 : }
1041 :
1042 : // prepare acceleration
1043 493 : Vector3f accel_ned_mss;
1044 493 : if (!acc_ignore) {
1045 440 : accel_ned_mss = Vector3f{packet.afx, packet.afy, packet.afz};
1046 : // rotate to earth-frame if necessary
1047 440 : if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
1048 0 : accel_ned_mss.xy() = copter.ahrs.body_to_earth2D(accel_ned_mss.xy());
1049 : }
1050 : }
1051 :
1052 : // prepare yaw
1053 493 : float yaw_rad = 0.0f;
1054 493 : bool yaw_relative = false;
1055 493 : float yaw_rate_rads = 0.0f;
1056 493 : if (!yaw_ignore) {
1057 0 : yaw_rad = packet.yaw;
1058 0 : yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
1059 : }
1060 493 : if (!yaw_rate_ignore) {
1061 0 : yaw_rate_rads = packet.yaw_rate;
1062 : }
1063 :
1064 : // send request
1065 493 : if (!pos_ignore && !vel_ignore) {
1066 1 : copter.mode_guided.set_pos_vel_accel_NED_m(pos_ned_m, vel_ned_ms, accel_ned_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative);
1067 492 : } else if (pos_ignore && !vel_ignore) {
1068 438 : copter.mode_guided.set_vel_accel_NED_m(vel_ned_ms, accel_ned_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative);
1069 54 : } else if (pos_ignore && vel_ignore && !acc_ignore) {
1070 1 : copter.mode_guided.set_accel_NED_mss(accel_ned_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative);
1071 53 : } else if (!pos_ignore && vel_ignore && acc_ignore) {
1072 53 : copter.mode_guided.set_pos_NED_m(pos_ned_m, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads, yaw_relative, false);
1073 : } else {
1074 : // unsupported combination of position/velocity/acceleration flags
1075 0 : copter.mode_guided.hold_position();
1076 : }
1077 : }
1078 :
1079 2842 : void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mavlink_message_t &msg)
1080 : {
1081 : // decode packet
1082 : mavlink_set_position_target_global_int_t packet;
1083 2842 : mavlink_msg_set_position_target_global_int_decode(&msg, &packet);
1084 :
1085 : // exit if vehicle is not in Guided mode or Auto-Guided mode
1086 2842 : if (!copter.flightmode->in_guided_mode()) {
1087 54 : return;
1088 : }
1089 :
1090 : // todo: do we need to check for supported coordinate frames
1091 :
1092 2842 : bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
1093 2842 : bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
1094 2842 : bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
1095 2842 : bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
1096 2842 : bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
1097 2842 : bool force_set = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE_SET;
1098 :
1099 : // Force inputs are not supported
1100 : // Do not accept command if force_set is true and acc_ignore is false
1101 2842 : if (force_set && !acc_ignore) {
1102 0 : copter.mode_guided.hold_position();
1103 0 : return;
1104 : }
1105 :
1106 : // extract location from message
1107 2842 : Location loc;
1108 2842 : if (!pos_ignore) {
1109 : // sanity check location
1110 413 : if (!check_latlng(packet.lat_int, packet.lon_int)) {
1111 : // invalid latitude or longitude
1112 0 : copter.mode_guided.hold_position();
1113 0 : return;
1114 : }
1115 : Location::AltFrame frame;
1116 413 : if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) {
1117 : // unknown coordinate frame
1118 0 : copter.mode_guided.hold_position();
1119 0 : return;
1120 : }
1121 413 : loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};
1122 : }
1123 :
1124 : // prepare velocity
1125 2842 : Vector3f vel_ned_ms;
1126 2842 : if (!vel_ignore) {
1127 2429 : vel_ned_ms = Vector3f{packet.vx, packet.vy, packet.vz};
1128 2429 : if (!sane_vel_or_acc_vector(vel_ned_ms)) {
1129 : // velocity vector contains NaN or Inf
1130 54 : copter.mode_guided.hold_position();
1131 54 : return;
1132 : }
1133 : }
1134 :
1135 : // prepare acceleration
1136 2788 : Vector3f accel_ned_mss;
1137 2788 : if (!acc_ignore) {
1138 0 : accel_ned_mss = Vector3f{packet.afx, packet.afy, packet.afz};
1139 : }
1140 :
1141 : // prepare yaw
1142 2788 : float yaw_rad = 0.0f;
1143 2788 : float yaw_rate_rads = 0.0f;
1144 2788 : if (!yaw_ignore) {
1145 906 : yaw_rad = packet.yaw;
1146 : }
1147 2788 : if (!yaw_rate_ignore) {
1148 1162 : yaw_rate_rads = packet.yaw_rate;
1149 : }
1150 :
1151 : // send targets to the appropriate guided mode controller
1152 2788 : if (!pos_ignore && !vel_ignore) {
1153 : // convert Location to vector from ekf origin for posvel controller
1154 0 : if (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) {
1155 : // posvel controller does not support alt-above-terrain
1156 0 : copter.mode_guided.hold_position();
1157 0 : return;
1158 : }
1159 0 : Vector3p pos_ned_m;
1160 0 : if (!loc.get_vector_from_origin_NED_m(pos_ned_m)) {
1161 : // could not convert location to NED position
1162 0 : copter.mode_guided.hold_position();
1163 0 : return;
1164 : }
1165 0 : copter.mode_guided.set_pos_vel_NED_m(pos_ned_m, vel_ned_ms, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads);
1166 2788 : } else if (pos_ignore && !vel_ignore) {
1167 2375 : copter.mode_guided.set_vel_accel_NED_m(vel_ned_ms, accel_ned_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads);
1168 413 : } else if (pos_ignore && vel_ignore && !acc_ignore) {
1169 0 : copter.mode_guided.set_accel_NED_mss(accel_ned_mss, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads);
1170 413 : } else if (!pos_ignore && vel_ignore && acc_ignore) {
1171 413 : copter.mode_guided.set_destination(loc, !yaw_ignore, yaw_rad, !yaw_rate_ignore, yaw_rate_rads);
1172 : } else {
1173 : // unsupported combination of position/velocity/acceleration flags
1174 0 : copter.mode_guided.hold_position();
1175 : }
1176 : }
1177 : #endif // MODE_GUIDED_ENABLED
1178 :
1179 1210893 : void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
1180 : {
1181 :
1182 1210893 : switch (msg.msgid) {
1183 : #if MODE_GUIDED_ENABLED
1184 115 : case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
1185 115 : handle_message_set_attitude_target(msg);
1186 115 : break;
1187 544 : case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
1188 544 : handle_message_set_position_target_local_ned(msg);
1189 544 : break;
1190 2842 : case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
1191 2842 : handle_message_set_position_target_global_int(msg);
1192 2842 : break;
1193 : #endif
1194 :
1195 : #if TOY_MODE_ENABLED
1196 : case MAVLINK_MSG_ID_NAMED_VALUE_INT:
1197 : copter.g2.toy_mode.handle_message(msg);
1198 : break;
1199 : #endif
1200 1207392 : default:
1201 1207392 : GCS_MAVLINK::handle_message(msg);
1202 1206586 : break;
1203 : }
1204 1210087 : }
1205 :
1206 2 : MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_int_t &packet) {
1207 : #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
1208 : if (GCS_MAVLINK::handle_flight_termination(packet) == MAV_RESULT_ACCEPTED) {
1209 : return MAV_RESULT_ACCEPTED;
1210 : }
1211 : #endif
1212 2 : if (packet.param1 > 0.5f) {
1213 2 : copter.arming.disarm(AP_Arming::Method::TERMINATION);
1214 2 : return MAV_RESULT_ACCEPTED;
1215 : }
1216 :
1217 0 : return MAV_RESULT_FAILED;
1218 : }
1219 :
1220 385992 : float GCS_MAVLINK_Copter::vfr_hud_alt() const
1221 : {
1222 385992 : if (copter.g2.dev_options.get() & DevOptionVFR_HUDRelativeAlt) {
1223 : // compatibility option for older mavlink-aware devices that
1224 : // assume Copter returns a relative altitude in VFR_HUD.alt
1225 0 : return copter.current_loc.alt * 0.01f;
1226 : }
1227 385992 : return GCS_MAVLINK::vfr_hud_alt();
1228 : }
1229 :
1230 6 : uint64_t GCS_MAVLINK_Copter::capabilities() const
1231 : {
1232 : return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
1233 : MAV_PROTOCOL_CAPABILITY_MISSION_INT |
1234 : MAV_PROTOCOL_CAPABILITY_COMMAND_INT |
1235 : MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
1236 : MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
1237 : MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION |
1238 6 : MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
1239 : #if AP_TERRAIN_AVAILABLE
1240 6 : (copter.terrain.enabled() ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0) |
1241 : #endif
1242 6 : GCS_MAVLINK::capabilities());
1243 : }
1244 :
1245 292 : MAV_LANDED_STATE GCS_MAVLINK_Copter::landed_state() const
1246 : {
1247 292 : if (copter.ap.land_complete) {
1248 179 : return MAV_LANDED_STATE_ON_GROUND;
1249 : }
1250 113 : if (copter.flightmode->is_landing()) {
1251 52 : return MAV_LANDED_STATE_LANDING;
1252 : }
1253 61 : if (copter.flightmode->is_taking_off()) {
1254 15 : return MAV_LANDED_STATE_TAKEOFF;
1255 : }
1256 46 : return MAV_LANDED_STATE_IN_AIR;
1257 : }
1258 :
1259 386167 : void GCS_MAVLINK_Copter::send_wind() const
1260 : {
1261 386167 : Vector3f airspeed_vec_bf;
1262 386167 : if (!AP::ahrs().airspeed_vector_TAS(airspeed_vec_bf)) {
1263 : // if we don't have an airspeed estimate then we don't have a
1264 : // valid wind estimate on copters
1265 126002 : return;
1266 : }
1267 260165 : Vector3f wind;
1268 : // send the estimate even if it is not marked valid, to preserve
1269 : // existing behaviour
1270 260165 : IGNORE_RETURN(AP::ahrs().get_wind(wind));
1271 260165 : mavlink_msg_wind_send(
1272 260165 : chan,
1273 260165 : degrees(atan2f(-wind.y, -wind.x)),
1274 260165 : wind.xy().length(),
1275 : wind.z);
1276 : }
1277 :
1278 : #if HAL_HIGH_LATENCY2_ENABLED
1279 0 : int16_t GCS_MAVLINK_Copter::high_latency_target_altitude() const
1280 : {
1281 0 : AP_AHRS &ahrs = AP::ahrs();
1282 0 : Location global_position_current;
1283 0 : UNUSED_RESULT(ahrs.get_location(global_position_current));
1284 :
1285 : //return units are m
1286 0 : if (copter.ap.initialised) {
1287 0 : return global_position_current.alt * 0.01 - copter.pos_control->get_pos_error_D_m();
1288 : }
1289 0 : return 0;
1290 :
1291 : }
1292 :
1293 0 : uint8_t GCS_MAVLINK_Copter::high_latency_tgt_heading() const
1294 : {
1295 0 : if (copter.ap.initialised) {
1296 : // return units are deg/2
1297 0 : const Mode *flightmode = copter.flightmode;
1298 : // need to convert -180->180 to 0->360/2
1299 0 : return wrap_360(flightmode->wp_bearing_deg()) * 0.5;
1300 : }
1301 0 : return 0;
1302 : }
1303 :
1304 0 : uint16_t GCS_MAVLINK_Copter::high_latency_tgt_dist_dam() const
1305 : {
1306 0 : if (copter.ap.initialised) {
1307 0 : const Mode *flightmode = copter.flightmode;
1308 0 : return MIN(static_cast<uint16_t>(flightmode->wp_distance_m() * 0.1), UINT16_MAX);
1309 : }
1310 0 : return 0;
1311 : }
1312 :
1313 0 : uint8_t GCS_MAVLINK_Copter::high_latency_tgt_airspeed() const
1314 : {
1315 0 : if (copter.ap.initialised) {
1316 : // return units are m/s*5
1317 0 : return MIN(copter.pos_control->get_vel_target_NED_ms().length() * 5.0, UINT8_MAX);
1318 : }
1319 0 : return 0;
1320 : }
1321 :
1322 0 : uint8_t GCS_MAVLINK_Copter::high_latency_wind_speed() const
1323 : {
1324 0 : Vector3f airspeed_vec_bf;
1325 0 : Vector3f wind;
1326 : // return units are m/s*5
1327 0 : if (AP::ahrs().airspeed_vector_TAS(airspeed_vec_bf)) {
1328 : // use the estimate even if it is not marked valid, to preserve
1329 : // existing behaviour
1330 0 : IGNORE_RETURN(AP::ahrs().get_wind(wind));
1331 0 : return wind.xy().length() * 5;
1332 : }
1333 0 : return 0;
1334 : }
1335 :
1336 0 : uint8_t GCS_MAVLINK_Copter::high_latency_wind_direction() const
1337 : {
1338 0 : Vector3f airspeed_vec_bf;
1339 0 : Vector3f wind;
1340 : // return units are deg/2
1341 0 : if (AP::ahrs().airspeed_vector_TAS(airspeed_vec_bf)) {
1342 : // use the estimate even if it is not marked valid, to preserve
1343 : // existing behaviour
1344 0 : IGNORE_RETURN(AP::ahrs().get_wind(wind));
1345 : // need to convert -180->180 to 0->360/2
1346 0 : return wrap_360(degrees(atan2f(-wind.y, -wind.x))) / 2;
1347 : }
1348 0 : return 0;
1349 : }
1350 : #endif // HAL_HIGH_LATENCY2_ENABLED
1351 :
1352 : // Send the mode with the given index (not mode number!) return the total number of modes
1353 : // Index starts at 1
1354 0 : uint8_t GCS_MAVLINK_Copter::send_available_mode(uint8_t index) const
1355 : {
1356 0 : const Mode* modes[] {
1357 : #if MODE_AUTO_ENABLED
1358 : &copter.mode_auto, // This auto is actually auto RTL!
1359 : &copter.mode_auto, // This one is really is auto!
1360 : #endif
1361 : #if MODE_ACRO_ENABLED
1362 : &copter.mode_acro,
1363 : #endif
1364 : &copter.mode_stabilize,
1365 : #if MODE_ALTHOLD_ENABLED
1366 : &copter.mode_althold,
1367 : #endif
1368 : #if MODE_CIRCLE_ENABLED
1369 : &copter.mode_circle,
1370 : #endif
1371 : #if MODE_LOITER_ENABLED
1372 : &copter.mode_loiter,
1373 : #endif
1374 : #if MODE_GUIDED_ENABLED
1375 : &copter.mode_guided,
1376 : #endif
1377 : &copter.mode_land,
1378 : #if MODE_RTL_ENABLED
1379 : &copter.mode_rtl,
1380 : #endif
1381 : #if MODE_DRIFT_ENABLED
1382 : &copter.mode_drift,
1383 : #endif
1384 : #if MODE_SPORT_ENABLED
1385 : &copter.mode_sport,
1386 : #endif
1387 : #if MODE_FLIP_ENABLED
1388 : &copter.mode_flip,
1389 : #endif
1390 : #if AUTOTUNE_ENABLED
1391 : &copter.mode_autotune,
1392 : #endif
1393 : #if MODE_POSHOLD_ENABLED
1394 : &copter.mode_poshold,
1395 : #endif
1396 : #if MODE_BRAKE_ENABLED
1397 : &copter.mode_brake,
1398 : #endif
1399 : #if MODE_THROW_ENABLED
1400 : &copter.mode_throw,
1401 : #endif
1402 : #if AP_ADSB_AVOIDANCE_ENABLED
1403 : &copter.mode_avoid_adsb,
1404 : #endif
1405 : #if MODE_GUIDED_NOGPS_ENABLED
1406 : &copter.mode_guided_nogps,
1407 : #endif
1408 : #if MODE_SMARTRTL_ENABLED
1409 : &copter.mode_smartrtl,
1410 : #endif
1411 : #if MODE_FLOWHOLD_ENABLED
1412 0 : (Mode*)copter.g2.mode_flowhold_ptr,
1413 : #endif
1414 : #if MODE_FOLLOW_ENABLED
1415 : &copter.mode_follow,
1416 : #endif
1417 : #if MODE_ZIGZAG_ENABLED
1418 : &copter.mode_zigzag,
1419 : #endif
1420 : #if MODE_SYSTEMID_ENABLED
1421 0 : (Mode *)copter.g2.mode_systemid_ptr,
1422 : #endif
1423 : #if MODE_AUTOROTATE_ENABLED
1424 : &copter.mode_autorotate,
1425 : #endif
1426 : #if MODE_TURTLE_ENABLED
1427 : &copter.mode_turtle,
1428 : #endif
1429 0 : };
1430 :
1431 0 : const uint8_t base_mode_count = ARRAY_SIZE(modes);
1432 0 : uint8_t mode_count = base_mode_count;
1433 :
1434 : #if AP_SCRIPTING_ENABLED
1435 0 : for (uint8_t i = 0; i < ARRAY_SIZE(copter.mode_guided_custom); i++) {
1436 0 : if (copter.mode_guided_custom[i] != nullptr) {
1437 0 : mode_count += 1;
1438 : }
1439 : }
1440 : #endif
1441 :
1442 : // Convert to zero indexed
1443 0 : const uint8_t index_zero = index - 1;
1444 0 : if (index_zero >= mode_count) {
1445 : // Mode does not exist!?
1446 0 : return mode_count;
1447 : }
1448 :
1449 : // Ask the mode for its name and number
1450 : const char* name;
1451 : Mode::Number mode_number;
1452 : bool enabled;
1453 :
1454 0 : if (index_zero < base_mode_count) {
1455 0 : name = modes[index_zero]->name();
1456 0 : mode_number = modes[index_zero]->mode_number();
1457 0 : enabled = modes[index_zero]->enabled();
1458 :
1459 : } else {
1460 : #if AP_SCRIPTING_ENABLED
1461 0 : const uint8_t custom_index = index_zero - base_mode_count;
1462 0 : if (copter.mode_guided_custom[custom_index] == nullptr) {
1463 : // Invalid index, should not happen
1464 0 : return mode_count;
1465 : }
1466 0 : name = copter.mode_guided_custom[custom_index]->name();
1467 0 : mode_number = copter.mode_guided_custom[custom_index]->mode_number();
1468 0 : enabled = true; // The script has actively added the mode, it must be enabled
1469 : #else
1470 : // Should not endup here
1471 : return mode_count;
1472 : #endif
1473 : }
1474 :
1475 : #if MODE_AUTO_ENABLED
1476 : // Auto RTL is odd
1477 : // Have to deal with is separately because its number and name can change depending on if were in it or not
1478 0 : if (index_zero == 0) {
1479 0 : mode_number = Mode::Number::AUTO_RTL;
1480 0 : name = "Auto RTL";
1481 :
1482 0 : } else if (index_zero == 1) {
1483 0 : mode_number = Mode::Number::AUTO;
1484 0 : name = "Auto";
1485 :
1486 : }
1487 : #endif
1488 :
1489 : // the check here must be the same as the one in `get_available_mode_enabled_mask`
1490 0 : const bool user_selectable = enabled && copter.gcs_mode_enabled(mode_number);
1491 :
1492 0 : mavlink_msg_available_modes_send(
1493 0 : chan,
1494 : mode_count,
1495 : index,
1496 : MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
1497 : (uint8_t)mode_number,
1498 : // MAV_MODE_PROPERTY bitmask,
1499 : user_selectable ? 0 : MAV_MODE_PROPERTY_NOT_USER_SELECTABLE,
1500 : name
1501 : );
1502 :
1503 0 : return mode_count;
1504 : }
|
