Line data Source code
1 : #include "GCS_MAVLink_Plane.h"
2 :
3 : #include "Plane.h"
4 : #include <AP_RPM/AP_RPM_config.h>
5 : #include <AP_Airspeed/AP_Airspeed_config.h>
6 : #include <AP_EFI/AP_EFI_config.h>
7 :
8 60885 : MAV_TYPE GCS_Plane::frame_type() const
9 : {
10 : #if HAL_QUADPLANE_ENABLED
11 60885 : return plane.quadplane.get_mav_type();
12 : #else
13 : return MAV_TYPE_FIXED_WING;
14 : #endif
15 : }
16 :
17 60877 : uint8_t GCS_MAVLINK_Plane::base_mode() const
18 : {
19 60877 : uint8_t _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
20 :
21 : // work out the base_mode. This value is not very useful
22 : // for APM, but we calculate it as best we can so a generic
23 : // MAVLink enabled ground station can work out something about
24 : // what the MAV is up to. The actual bit values are highly
25 : // ambiguous for most of the APM flight modes. In practice, you
26 : // only get useful information from the custom_mode, which maps to
27 : // the APM flight mode and has a well defined meaning in the
28 : // ArduPlane documentation
29 60877 : switch (plane.control_mode->mode_number()) {
30 10713 : case Mode::Number::MANUAL:
31 : case Mode::Number::TRAINING:
32 : case Mode::Number::ACRO:
33 : #if HAL_QUADPLANE_ENABLED
34 : case Mode::Number::QACRO:
35 10713 : _base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
36 10713 : break;
37 : #endif
38 11698 : case Mode::Number::STABILIZE:
39 : case Mode::Number::FLY_BY_WIRE_A:
40 : case Mode::Number::AUTOTUNE:
41 : case Mode::Number::FLY_BY_WIRE_B:
42 : #if HAL_QUADPLANE_ENABLED
43 : case Mode::Number::QSTABILIZE:
44 : case Mode::Number::QHOVER:
45 : case Mode::Number::QLOITER:
46 : case Mode::Number::QLAND:
47 : #if QAUTOTUNE_ENABLED
48 : case Mode::Number::QAUTOTUNE:
49 : #endif
50 : #endif // HAL_QUADPLANE_ENABLED
51 : case Mode::Number::CRUISE:
52 11698 : _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
53 11698 : break;
54 36585 : case Mode::Number::AUTO:
55 : case Mode::Number::RTL:
56 : case Mode::Number::LOITER:
57 : case Mode::Number::THERMAL:
58 : case Mode::Number::AVOID_ADSB:
59 : case Mode::Number::GUIDED:
60 : case Mode::Number::CIRCLE:
61 : case Mode::Number::TAKEOFF:
62 : #if MODE_AUTOLAND_ENABLED
63 : case Mode::Number::AUTOLAND:
64 : #endif
65 : #if HAL_QUADPLANE_ENABLED
66 : case Mode::Number::QRTL:
67 : case Mode::Number::LOITER_ALT_QLAND:
68 : #endif
69 36585 : _base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
70 : MAV_MODE_FLAG_STABILIZE_ENABLED;
71 : // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
72 : // APM does in any mode, as that is defined as "system finds its own goal
73 : // positions", which APM does not currently do
74 36585 : break;
75 1881 : case Mode::Number::INITIALISING:
76 1881 : break;
77 : }
78 :
79 60877 : if (!plane.training_manual_pitch || !plane.training_manual_roll) {
80 60329 : _base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
81 : }
82 :
83 60877 : if (plane.control_mode != &plane.mode_manual && plane.control_mode != &plane.mode_initializing) {
84 : // stabiliser of some form is enabled
85 48361 : _base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
86 : }
87 :
88 60877 : if (plane.g.stick_mixing != StickMixing::NONE && plane.control_mode != &plane.mode_initializing) {
89 58782 : if ((plane.g.stick_mixing != StickMixing::VTOL_YAW) || (plane.control_mode == &plane.mode_auto)) {
90 : // all modes except INITIALISING have some form of manual
91 : // override if stick mixing is enabled
92 58782 : _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
93 : }
94 : }
95 :
96 : // we are armed if we are not initialising
97 60877 : if (plane.control_mode != &plane.mode_initializing && plane.arming.is_armed()) {
98 42165 : _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
99 : }
100 :
101 : // indicate we have set a custom mode
102 60877 : _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
103 :
104 60877 : return _base_mode;
105 : }
106 :
107 61020 : uint32_t GCS_Plane::custom_mode() const
108 : {
109 61020 : return plane.control_mode->mode_number();
110 : }
111 :
112 60148 : MAV_STATE GCS_MAVLINK_Plane::vehicle_system_status() const
113 : {
114 60148 : if (plane.control_mode == &plane.mode_initializing) {
115 1881 : return MAV_STATE_CALIBRATING;
116 : }
117 58267 : if (plane.any_failsafe_triggered()) {
118 197 : return MAV_STATE_CRITICAL;
119 : }
120 58070 : if (plane.crash_state.is_crashed) {
121 0 : return MAV_STATE_EMERGENCY;
122 : }
123 58070 : if (plane.is_flying()) {
124 39816 : return MAV_STATE_ACTIVE;
125 : }
126 :
127 18254 : return MAV_STATE_STANDBY;
128 : }
129 :
130 :
131 552408 : void GCS_MAVLINK_Plane::send_attitude() const
132 : {
133 552408 : const AP_AHRS &ahrs = AP::ahrs();
134 :
135 552408 : float r = ahrs.get_roll_rad();
136 552408 : float p = ahrs.get_pitch_rad();
137 552408 : float y = ahrs.get_yaw_rad();
138 :
139 552408 : if (!(plane.flight_option_enabled(FlightOptions::GCS_REMOVE_TRIM_PITCH))) {
140 552408 : p -= radians(plane.g.pitch_trim);
141 : }
142 :
143 : #if HAL_QUADPLANE_ENABLED
144 552408 : if (plane.quadplane.show_vtol_view()) {
145 91693 : r = plane.quadplane.ahrs_view->roll;
146 91693 : p = plane.quadplane.ahrs_view->pitch;
147 91693 : y = plane.quadplane.ahrs_view->yaw;
148 : }
149 : #endif
150 :
151 552408 : const Vector3f &omega = ahrs.get_gyro();
152 1104816 : mavlink_msg_attitude_send(
153 552408 : chan,
154 : millis(),
155 : r,
156 : p,
157 : y,
158 552408 : omega.x,
159 552408 : omega.y,
160 552408 : omega.z);
161 552408 : }
162 :
163 0 : void GCS_MAVLINK_Plane::send_attitude_target()
164 : {
165 : #if HAL_QUADPLANE_ENABLED
166 : // Check if the attitude target is valid for reporting
167 0 : const uint32_t now = AP_HAL::millis();
168 0 : if (now - plane.quadplane.last_att_control_ms > 100) {
169 0 : return;
170 : }
171 :
172 0 : const Quaternion quat = plane.quadplane.attitude_control->get_attitude_target_quat();
173 0 : const Vector3f& ang_vel = plane.quadplane.attitude_control->get_attitude_target_ang_vel();
174 0 : const float throttle = plane.quadplane.attitude_control->get_throttle_in();
175 :
176 0 : const float quat_out[4] {quat.q1, quat.q2, quat.q3, quat.q4};
177 :
178 0 : const uint16_t typemask = 0;
179 :
180 0 : mavlink_msg_attitude_target_send(
181 : chan,
182 : now, // time since boot (ms)
183 : typemask, // Bitmask that tells the system what control dimensions should be ignored by the vehicle
184 : quat_out, // Target attitude quaternion [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], unit-length
185 0 : ang_vel.x, // bodyframe target roll rate (rad/s)
186 0 : ang_vel.y, // bodyframe target pitch rate (rad/s)
187 0 : ang_vel.z, // bodyframe yaw rate (rad/s)
188 : throttle); // Collective thrust, normalized to 0 .. 1
189 :
190 : #endif // HAL_QUADPLANE_ENABLED
191 : }
192 :
193 552407 : void GCS_MAVLINK_Plane::send_aoa_ssa()
194 : {
195 552407 : AP_AHRS &ahrs = AP::ahrs();
196 :
197 1104814 : mavlink_msg_aoa_ssa_send(
198 : chan,
199 552407 : micros(),
200 : ahrs.getAOA(),
201 : ahrs.getSSA());
202 552407 : }
203 :
204 552407 : void GCS_MAVLINK_Plane::send_nav_controller_output() const
205 : {
206 552407 : if (plane.control_mode == &plane.mode_manual) {
207 86132 : return;
208 : }
209 : #if HAL_QUADPLANE_ENABLED
210 466275 : const QuadPlane &quadplane = plane.quadplane;
211 466275 : if (quadplane.show_vtol_view() && quadplane.using_wp_nav()) {
212 24800 : const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
213 :
214 24800 : const Vector2f& curr_pos_m = quadplane.inertial_nav.get_position_xy_cm() * 0.01;
215 24800 : const Vector2f& target_pos_m = quadplane.pos_control->get_pos_target_NED_m().xy().tofloat();
216 24800 : const Vector2f error_m = (target_pos_m - curr_pos_m);
217 :
218 24800 : mavlink_msg_nav_controller_output_send(
219 24800 : chan,
220 24800 : targets.x * 0.01,
221 24800 : targets.y * 0.01,
222 24800 : targets.z * 0.01,
223 24800 : degrees(error_m.angle()),
224 24800 : MIN(error_m.length(), UINT16_MAX),
225 24800 : (plane.control_mode != &plane.mode_qstabilize) ? -quadplane.pos_control->get_pos_error_D_m() : 0,
226 24800 : plane.airspeed_error * 100, // incorrect units; see PR#7933
227 24800 : quadplane.wp_nav->crosstrack_error_m());
228 24800 : return;
229 : }
230 : #endif
231 : {
232 441475 : const AP_Navigation *nav_controller = plane.nav_controller;
233 2648850 : mavlink_msg_nav_controller_output_send(
234 441475 : chan,
235 441475 : plane.nav_roll_cd * 0.01,
236 441475 : plane.nav_pitch_cd * 0.01,
237 441475 : nav_controller->nav_bearing_cd() * 0.01,
238 441475 : nav_controller->target_bearing_cd() * 0.01,
239 882950 : MIN(plane.auto_state.wp_distance, UINT16_MAX),
240 441475 : plane.calc_altitude_error_cm() * 0.01,
241 441475 : plane.airspeed_error * 100, // incorrect units; see PR#7933
242 441475 : nav_controller->crosstrack_error_m());
243 : }
244 : }
245 :
246 552411 : bool GCS_MAVLINK_Plane::get_target_location(Location &loc) const
247 : {
248 552411 : if (plane.control_mode == &plane.mode_manual) {
249 86133 : return false;
250 : }
251 466278 : loc = plane.next_WP_loc;
252 466278 : return true;
253 : }
254 :
255 :
256 553136 : float GCS_MAVLINK_Plane::vfr_hud_airspeed() const
257 : {
258 : // airspeed sensors are best. While the AHRS airspeed_estimate
259 : // will use an airspeed sensor, that value is constrained by the
260 : // ground speed. When reporting we should send the true airspeed
261 : // value if possible:
262 : #if AP_AIRSPEED_ENABLED
263 553136 : if (plane.airspeed.enabled() && plane.airspeed.healthy()) {
264 549150 : return plane.airspeed.get_airspeed();
265 : }
266 : #endif
267 :
268 : // airspeed estimates are OK:
269 : float aspeed;
270 3986 : if (AP::ahrs().airspeed_EAS(aspeed)) {
271 177 : return aspeed;
272 : }
273 :
274 : // lying is worst:
275 3809 : return 0;
276 : }
277 :
278 553156 : int16_t GCS_MAVLINK_Plane::vfr_hud_throttle() const
279 : {
280 553156 : return plane.throttle_percentage();
281 : }
282 :
283 552407 : float GCS_MAVLINK_Plane::vfr_hud_climbrate() const
284 : {
285 : #if HAL_SOARING_ENABLED
286 552407 : if (plane.g2.soaring_controller.is_active()) {
287 13750 : return plane.g2.soaring_controller.get_vario_reading();
288 : }
289 : #endif
290 538657 : return GCS_MAVLINK::vfr_hud_climbrate();
291 : }
292 :
293 552407 : void GCS_MAVLINK_Plane::send_wind() const
294 : {
295 552407 : Vector3f wind;
296 : // send the estimate even if it is not marked valid, to preserve
297 : // existing behaviour
298 552407 : IGNORE_RETURN(AP::ahrs().get_wind(wind));
299 552407 : mavlink_msg_wind_send(
300 552407 : chan,
301 552407 : degrees(atan2f(-wind.y, -wind.x)), // use negative, to give
302 : // direction wind is coming from
303 552407 : wind.xy().length(), // ground-plane (horizontal) speed only
304 : wind.z);
305 552407 : }
306 :
307 : // sends a single pid info over the provided channel
308 17 : void GCS_MAVLINK_Plane::send_pid_info(const AP_PIDInfo *pid_info,
309 : const uint8_t axis, const float achieved)
310 : {
311 17 : if (pid_info == nullptr) {
312 0 : return;
313 : }
314 17 : if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
315 0 : return;
316 : }
317 17 : mavlink_msg_pid_tuning_send(chan, axis,
318 17 : pid_info->target,
319 : achieved,
320 17 : pid_info->FF,
321 17 : pid_info->P,
322 17 : pid_info->I,
323 17 : pid_info->D,
324 17 : pid_info->slew_rate,
325 17 : pid_info->Dmod);
326 : }
327 :
328 : /*
329 : send PID tuning message
330 : */
331 552407 : void GCS_MAVLINK_Plane::send_pid_tuning()
332 : {
333 552407 : if (plane.control_mode == &plane.mode_manual) {
334 : // no PIDs should be used in manual
335 86132 : return;
336 : }
337 :
338 466275 : const Parameters &g = plane.g;
339 :
340 : const AP_PIDInfo *pid_info;
341 466275 : if (g.gcs_pid_mask & TUNING_BITS_ROLL) {
342 17 : pid_info = &plane.rollController.get_pid_info();
343 : #if HAL_QUADPLANE_ENABLED
344 17 : if (plane.quadplane.in_vtol_mode()) {
345 0 : pid_info = &plane.quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
346 : }
347 : #endif
348 17 : send_pid_info(pid_info, PID_TUNING_ROLL, pid_info->actual);
349 : }
350 466275 : if (g.gcs_pid_mask & TUNING_BITS_PITCH) {
351 0 : pid_info = &plane.pitchController.get_pid_info();
352 : #if HAL_QUADPLANE_ENABLED
353 0 : if (plane.quadplane.in_vtol_mode()) {
354 0 : pid_info = &plane.quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
355 : }
356 : #endif
357 0 : send_pid_info(pid_info, PID_TUNING_PITCH, pid_info->actual);
358 : }
359 466275 : if (g.gcs_pid_mask & TUNING_BITS_YAW) {
360 0 : pid_info = &plane.yawController.get_pid_info();
361 : #if HAL_QUADPLANE_ENABLED
362 0 : if (plane.quadplane.in_vtol_mode()) {
363 0 : pid_info = &plane.quadplane.attitude_control->get_rate_yaw_pid().get_pid_info();
364 : }
365 : #endif
366 0 : send_pid_info(pid_info, PID_TUNING_YAW, pid_info->actual);
367 : }
368 466275 : if (g.gcs_pid_mask & TUNING_BITS_STEER) {
369 0 : pid_info = &plane.steerController.get_pid_info();
370 0 : send_pid_info(pid_info, PID_TUNING_STEER, pid_info->actual);
371 : }
372 466275 : if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (plane.flight_stage == AP_FixedWing::FlightStage::LAND)) {
373 0 : AP_AHRS &ahrs = AP::ahrs();
374 0 : const Vector3f &gyro = ahrs.get_gyro();
375 0 : send_pid_info(plane.landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z));
376 : }
377 : #if HAL_QUADPLANE_ENABLED
378 466275 : if (g.gcs_pid_mask & TUNING_BITS_ACCZ && plane.quadplane.in_vtol_mode()) {
379 0 : pid_info = &plane.quadplane.pos_control->D_get_accel_pid().get_pid_info();
380 0 : send_pid_info(pid_info, PID_TUNING_ACCZ, pid_info->actual);
381 : }
382 : #endif
383 : }
384 :
385 : // try to send a message, return false if it won't fit in the serial tx buffer
386 46343812 : bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
387 : {
388 46343812 : switch (id) {
389 :
390 552407 : case MSG_WIND:
391 552407 : CHECK_PAYLOAD_SIZE(WIND);
392 552407 : send_wind();
393 552407 : break;
394 :
395 553322 : case MSG_ADSB_VEHICLE:
396 : #if HAL_ADSB_ENABLED
397 553322 : CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
398 553322 : plane.adsb.send_adsb_vehicle(chan);
399 : #endif
400 553322 : break;
401 :
402 552407 : case MSG_AOA_SSA:
403 552407 : CHECK_PAYLOAD_SIZE(AOA_SSA);
404 552407 : send_aoa_ssa();
405 552407 : break;
406 :
407 : #if HAL_LANDING_DEEPSTALL_ENABLED
408 552407 : case MSG_LANDING:
409 552407 : plane.landing.send_landing_message(chan);
410 552407 : break;
411 : #endif // HAL_LANDING_DEEPSTALL_ENABLED
412 :
413 552407 : case MSG_HYGROMETER:
414 : #if AP_AIRSPEED_HYGROMETER_ENABLE
415 552407 : CHECK_PAYLOAD_SIZE(HYGROMETER_SENSOR);
416 552407 : send_hygrometer();
417 : #endif
418 552407 : break;
419 :
420 43580862 : default:
421 43580862 : return GCS_MAVLINK::try_send_message(id);
422 : }
423 2762950 : return true;
424 : }
425 :
426 : #if AP_AIRSPEED_HYGROMETER_ENABLE
427 552407 : void GCS_MAVLINK_Plane::send_hygrometer()
428 : {
429 552407 : if (!HAVE_PAYLOAD_SPACE(chan, HYGROMETER_SENSOR)) {
430 0 : return;
431 : }
432 :
433 552407 : const auto *airspeed = AP::airspeed();
434 552407 : if (airspeed == nullptr) {
435 0 : return;
436 : }
437 552407 : const uint32_t now = AP_HAL::millis();
438 :
439 1657221 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
440 1104814 : uint8_t idx = (i+last_hygrometer_send_idx+1) % AIRSPEED_MAX_SENSORS;
441 : float temperature, humidity;
442 : uint32_t last_sample_ms;
443 1104814 : if (!airspeed->get_hygrometer(idx, last_sample_ms, temperature, humidity)) {
444 1104814 : continue;
445 : }
446 0 : if (now - last_sample_ms > 2000) {
447 : // not updating, stop sending
448 0 : continue;
449 : }
450 0 : if (!HAVE_PAYLOAD_SPACE(chan, HYGROMETER_SENSOR)) {
451 0 : return;
452 : }
453 :
454 0 : mavlink_msg_hygrometer_sensor_send(
455 : chan,
456 : idx,
457 0 : int16_t(temperature*100),
458 0 : uint16_t(humidity*100));
459 0 : last_hygrometer_send_idx = idx;
460 : }
461 : }
462 : #endif // AP_AIRSPEED_HYGROMETER_ENABLE
463 :
464 : /*
465 : handle a request to switch to guided mode. This happens via a
466 : callback from handle_mission_item()
467 : */
468 2 : bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd)
469 : {
470 2 : return plane.control_mode->handle_guided_request(cmd.content.location);
471 : }
472 :
473 : /*
474 : handle a request to change current WP altitude. This happens via a
475 : callback from handle_mission_item()
476 : */
477 7 : void GCS_MAVLINK_Plane::handle_change_alt_request(Location &location)
478 : {
479 7 : plane.fix_terrain_WP(location, __AP_LINE__);
480 :
481 7 : if (location.terrain_alt) {
482 1 : plane.next_WP_loc.copy_alt_from(location);
483 : } else {
484 : // convert to absolute alt
485 : float abs_alt_m;
486 6 : if (location.get_alt_m(Location::AltFrame::ABSOLUTE, abs_alt_m)) {
487 6 : plane.next_WP_loc.set_alt_m(abs_alt_m, Location::AltFrame::ABSOLUTE);
488 : }
489 : }
490 7 : plane.reset_offset_altitude();
491 7 : }
492 :
493 :
494 : /*
495 : handle a LANDING_TARGET command. The timestamp has been jitter corrected
496 : */
497 0 : void GCS_MAVLINK_Plane::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
498 : {
499 : #if AC_PRECLAND_ENABLED
500 0 : plane.g2.precland.handle_msg(packet, timestamp_ms);
501 : #endif
502 0 : }
503 :
504 41 : MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
505 : {
506 41 : plane.in_calibration = true;
507 41 : MAV_RESULT ret = GCS_MAVLINK::handle_command_preflight_calibration(packet, msg);
508 41 : plane.in_calibration = false;
509 :
510 41 : return ret;
511 : }
512 :
513 1145832 : void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status,
514 : const mavlink_message_t &msg)
515 : {
516 : #if AP_ADSB_AVOIDANCE_ENABLED
517 1145832 : plane.avoidance_adsb.handle_msg(msg);
518 : #endif
519 1145832 : GCS_MAVLINK::packetReceived(status, msg);
520 1145832 : }
521 :
522 :
523 4 : bool Plane::set_home_to_current_location(bool _lock)
524 : {
525 4 : if (!set_home_persistently(AP::gps().location())) {
526 0 : return false;
527 : }
528 4 : if (_lock) {
529 4 : AP::ahrs().lock_home();
530 : }
531 4 : if ((control_mode == &mode_rtl)
532 : #if HAL_QUADPLANE_ENABLED
533 4 : || (control_mode == &mode_qrtl)
534 : #endif
535 : ) {
536 : // if in RTL head to the updated home location
537 0 : control_mode->enter();
538 : }
539 4 : return true;
540 : }
541 47 : bool Plane::set_home(const Location& loc, bool _lock)
542 : {
543 47 : if (!AP::ahrs().set_home(loc)) {
544 0 : return false;
545 : }
546 47 : if (_lock) {
547 47 : AP::ahrs().lock_home();
548 : }
549 47 : if ((control_mode == &mode_rtl)
550 : #if HAL_QUADPLANE_ENABLED
551 47 : || (control_mode == &mode_qrtl)
552 : #endif
553 : ) {
554 : // if in RTL head to the updated home location
555 0 : control_mode->enter();
556 : }
557 47 : return true;
558 : }
559 :
560 59 : MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
561 : {
562 59 : Location requested_position;
563 59 : if (!location_from_command_t(packet, requested_position)) {
564 0 : return MAV_RESULT_DENIED;
565 : }
566 59 : plane.fix_terrain_WP(requested_position, __AP_LINE__);
567 :
568 59 : if (isnan(packet.param4) || is_zero(packet.param4)) {
569 54 : requested_position.loiter_ccw = 0;
570 : } else {
571 5 : requested_position.loiter_ccw = 1;
572 : }
573 :
574 59 : if (requested_position.sanitize(plane.current_loc)) {
575 : // if the location wasn't already sane don't load it
576 0 : return MAV_RESULT_DENIED;
577 : }
578 :
579 : #if AP_FENCE_ENABLED
580 : // reject destination if outside the fence
581 59 : if (!plane.fence.check_location_within_fence(requested_position)) {
582 1 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
583 1 : return MAV_RESULT_FAILED;
584 : }
585 : #endif
586 :
587 : // location is valid load and set
588 58 : if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
589 49 : (plane.control_mode == &plane.mode_guided)) {
590 58 : plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND);
591 : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
592 58 : plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
593 : #endif
594 :
595 : // convert to absolute alt
596 58 : if (!requested_position.terrain_alt) {
597 32 : requested_position.change_alt_frame(Location::AltFrame::ABSOLUTE);
598 : }
599 :
600 58 : plane.set_guided_WP(requested_position);
601 :
602 : // Loiter radius for planes. Positive radius in meters, direction is controlled by Yaw (param4) value, parsed above
603 58 : if (!isnan(packet.param3) && packet.param3 > 0) {
604 6 : plane.mode_guided.set_radius_and_direction(packet.param3, requested_position.loiter_ccw);
605 : }
606 :
607 58 : return MAV_RESULT_ACCEPTED;
608 : }
609 0 : return MAV_RESULT_FAILED;
610 : }
611 :
612 5 : MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_DO_CHANGE_ALTITUDE(const mavlink_command_int_t &packet)
613 : {
614 5 : const float alt = packet.param1;
615 5 : MAV_FRAME mav_frame = (MAV_FRAME)packet.param2;
616 : Location::AltFrame alt_frame;
617 5 : if (!mavlink_coordinate_frame_to_location_alt_frame(mav_frame, alt_frame)) {
618 0 : return MAV_RESULT_DENIED;
619 : }
620 : Location loc {
621 : 0,
622 : 0,
623 5 : int32_t(alt * 100), // m -> cm
624 : alt_frame,
625 5 : };
626 5 : handle_change_alt_request(loc);
627 5 : return MAV_RESULT_ACCEPTED;
628 : }
629 :
630 : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
631 : // these are GUIDED mode commands that are RATE or slew enabled, so you can have more powerful control than default controls.
632 15 : MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet)
633 : {
634 15 : switch(packet.command) {
635 4 : case MAV_CMD_GUIDED_CHANGE_SPEED: {
636 : // command is only valid in guided mode
637 4 : if (plane.control_mode != &plane.mode_guided) {
638 0 : return MAV_RESULT_FAILED;
639 : }
640 :
641 : // only airspeed commands are supported right now...
642 4 : if (int(packet.param1) != SPEED_TYPE_AIRSPEED) { // since SPEED_TYPE is int in range 0-1 and packet.param1 is a *float* this works.
643 0 : return MAV_RESULT_DENIED;
644 : }
645 :
646 4 : if (!plane.mode_guided.handle_change_airspeed(packet.param2, packet.param3)) {
647 0 : return MAV_RESULT_FAILED;
648 : }
649 4 : return MAV_RESULT_ACCEPTED;
650 : }
651 :
652 11 : case MAV_CMD_GUIDED_CHANGE_ALTITUDE: {
653 : // command is only valid in guided
654 11 : if (plane.control_mode != &plane.mode_guided) {
655 0 : return MAV_RESULT_FAILED;
656 : }
657 :
658 : // disallow default value of -1 and dangerous value of zero
659 11 : if (is_equal(packet.z, -1.0f) || is_equal(packet.z, 0.0f)){
660 0 : return MAV_RESULT_DENIED;
661 : }
662 :
663 : Location::AltFrame new_target_alt_frame;
664 11 : if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.frame, new_target_alt_frame)) {
665 0 : return MAV_RESULT_DENIED;
666 : }
667 : // keep a copy of what came in via MAVLink - this is needed for logging, but not for anything else
668 11 : plane.guided_state.target_mav_frame = packet.frame;
669 :
670 11 : const int32_t new_target_alt_cm = packet.z * 100;
671 11 : plane.guided_state.target_location.set_alt_cm(new_target_alt_cm, new_target_alt_frame);
672 11 : plane.guided_state.target_alt_time_ms = AP_HAL::millis();
673 :
674 : // param3 contains the desired vertical velocity (not acceleration)
675 11 : if (is_zero(packet.param3)) {
676 : // the user wanted /maximum altitude change rate, pick a large value as close enough
677 5 : plane.guided_state.target_alt_rate = 1000.0;
678 : } else {
679 6 : plane.guided_state.target_alt_rate = fabsf(packet.param3);
680 : }
681 :
682 11 : return MAV_RESULT_ACCEPTED;
683 : }
684 :
685 0 : case MAV_CMD_GUIDED_CHANGE_HEADING: {
686 :
687 : // command is only valid in guided mode
688 0 : if (plane.control_mode != &plane.mode_guided) {
689 0 : return MAV_RESULT_FAILED;
690 : }
691 :
692 : // don't accept packets outside of [0-360] degree range
693 0 : if (packet.param2 < 0.0f || packet.param2 >= 360.0f) {
694 0 : return MAV_RESULT_DENIED;
695 : }
696 :
697 0 : float new_target_heading = radians(wrap_180(packet.param2));
698 :
699 0 : switch(HEADING_TYPE(packet.param1)) {
700 0 : case HEADING_TYPE_COURSE_OVER_GROUND:
701 : // course over ground
702 0 : plane.guided_state.target_heading_type = GUIDED_HEADING_COG;
703 0 : plane.prev_WP_loc = plane.current_loc;
704 0 : break;
705 0 : case HEADING_TYPE_HEADING:
706 : // normal vehicle heading
707 0 : plane.guided_state.target_heading_type = GUIDED_HEADING_HEADING;
708 0 : break;
709 0 : case HEADING_TYPE_DEFAULT:
710 0 : plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
711 0 : return MAV_RESULT_ACCEPTED;
712 0 : default:
713 : // MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters).
714 0 : return MAV_RESULT_DENIED;
715 : }
716 :
717 0 : plane.g2.guidedHeading.reset_I();
718 :
719 0 : plane.guided_state.target_heading = new_target_heading;
720 0 : plane.guided_state.target_heading_accel_limit = MAX(packet.param3, 0.05f);
721 0 : plane.guided_state.target_heading_time_ms = AP_HAL::millis();
722 0 : return MAV_RESULT_ACCEPTED;
723 : }
724 : }
725 : // anything else ...
726 0 : return MAV_RESULT_UNSUPPORTED;
727 : }
728 : #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
729 :
730 214727 : MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
731 : {
732 214727 : switch(packet.command) {
733 :
734 2 : case MAV_CMD_DO_AUTOTUNE_ENABLE:
735 2 : return handle_MAV_CMD_DO_AUTOTUNE_ENABLE(packet);
736 :
737 59 : case MAV_CMD_DO_REPOSITION:
738 59 : return handle_command_int_do_reposition(packet);
739 :
740 5 : case MAV_CMD_DO_CHANGE_ALTITUDE:
741 5 : return handle_command_int_DO_CHANGE_ALTITUDE(packet);
742 :
743 : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
744 : // special 'slew-enabled' guided commands here... for speed,alt, and direction commands
745 15 : case MAV_CMD_GUIDED_CHANGE_SPEED:
746 : case MAV_CMD_GUIDED_CHANGE_ALTITUDE:
747 : case MAV_CMD_GUIDED_CHANGE_HEADING:
748 15 : return handle_command_int_guided_slew_commands(packet);
749 : #endif
750 :
751 : #if AP_ICENGINE_ENABLED
752 15 : case MAV_CMD_DO_ENGINE_CONTROL:
753 15 : if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3, (uint32_t)packet.param4)) {
754 4 : return MAV_RESULT_FAILED;
755 : }
756 11 : return MAV_RESULT_ACCEPTED;
757 : #endif
758 :
759 28 : case MAV_CMD_DO_CHANGE_SPEED:
760 28 : return handle_command_DO_CHANGE_SPEED(packet);
761 :
762 : #if HAL_PARACHUTE_ENABLED
763 2 : case MAV_CMD_DO_PARACHUTE:
764 2 : return handle_MAV_CMD_DO_PARACHUTE(packet);
765 : #endif
766 :
767 : #if HAL_QUADPLANE_ENABLED
768 4 : case MAV_CMD_DO_MOTOR_TEST:
769 4 : return handle_MAV_CMD_DO_MOTOR_TEST(packet);
770 :
771 4 : case MAV_CMD_DO_VTOL_TRANSITION:
772 4 : return handle_command_DO_VTOL_TRANSITION(packet);
773 :
774 12 : case MAV_CMD_NAV_TAKEOFF:
775 12 : return handle_command_MAV_CMD_NAV_TAKEOFF(packet);
776 : #endif
777 :
778 2 : case MAV_CMD_DO_GO_AROUND:
779 2 : return plane.trigger_land_abort(packet.param1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
780 :
781 0 : case MAV_CMD_DO_RETURN_PATH_START:
782 : // attempt to rejoin after the next DO_RETURN_PATH_START command in the mission
783 0 : if (plane.have_position && plane.mission.jump_to_closest_mission_leg(plane.current_loc)) {
784 0 : plane.mission.set_force_resume(true);
785 0 : if (plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND)) {
786 0 : return MAV_RESULT_ACCEPTED;
787 : }
788 : // mode change failed, revert force resume flag
789 0 : plane.mission.set_force_resume(false);
790 : }
791 0 : return MAV_RESULT_FAILED;
792 :
793 2 : case MAV_CMD_DO_LAND_START:
794 : // attempt to switch to next DO_LAND_START command in the mission
795 2 : if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) {
796 2 : plane.mission.set_force_resume(true);
797 2 : if (plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND)) {
798 2 : return MAV_RESULT_ACCEPTED;
799 : }
800 : // mode change failed, revert force resume flag
801 0 : plane.mission.set_force_resume(false);
802 : }
803 0 : return MAV_RESULT_FAILED;
804 :
805 7 : case MAV_CMD_MISSION_START:
806 7 : if (!is_zero(packet.param1) || !is_zero(packet.param2)) {
807 : // first-item/last item not supported
808 0 : return MAV_RESULT_DENIED;
809 : }
810 7 : plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND);
811 7 : return MAV_RESULT_ACCEPTED;
812 :
813 2 : case MAV_CMD_NAV_LOITER_UNLIM:
814 2 : plane.set_mode(plane.mode_loiter, ModeReason::GCS_COMMAND);
815 2 : return MAV_RESULT_ACCEPTED;
816 :
817 2 : case MAV_CMD_NAV_RETURN_TO_LAUNCH:
818 2 : plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND);
819 2 : return MAV_RESULT_ACCEPTED;
820 :
821 : #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
822 0 : case MAV_CMD_SET_HAGL:
823 0 : plane.handle_external_hagl(packet);
824 0 : return MAV_RESULT_ACCEPTED;
825 : #endif
826 :
827 214566 : default:
828 214566 : return GCS_MAVLINK::handle_command_int_packet(packet, msg);
829 : }
830 : }
831 :
832 28 : MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_command_int_t &packet)
833 : {
834 : // if we're in failsafe modes (e.g., RTL, LOITER) or in pilot
835 : // controlled modes (e.g., MANUAL, TRAINING)
836 : // this command should be ignored since it comes in from GCS
837 : // or a companion computer:
838 29 : if ((!plane.control_mode->is_guided_mode()) &&
839 1 : (plane.control_mode != &plane.mode_auto)) {
840 : // failed
841 0 : return MAV_RESULT_FAILED;
842 : }
843 :
844 28 : if (plane.do_change_speed((SPEED_TYPE)packet.param1, packet.param2, packet.param3)) {
845 28 : return MAV_RESULT_ACCEPTED;
846 : }
847 0 : return MAV_RESULT_FAILED;
848 : }
849 :
850 : #if HAL_QUADPLANE_ENABLED
851 : #if AP_MAVLINK_COMMAND_LONG_ENABLED
852 24 : void GCS_MAVLINK_Plane::convert_MAV_CMD_NAV_TAKEOFF_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out)
853 : {
854 : // convert to MAV_FRAME_LOCAL_OFFSET_NED, "NED local tangent frame
855 : // with origin that travels with the vehicle"
856 24 : out = {};
857 24 : out.target_system = in.target_system;
858 24 : out.target_component = in.target_component;
859 24 : out.frame = MAV_FRAME_LOCAL_OFFSET_NED;
860 24 : out.command = in.command;
861 : // out.current = 0;
862 : // out.autocontinue = 0;
863 : // out.param1 = in.param1; // we only use the "z" parameter in this command:
864 : // out.param2 = in.param2;
865 : // out.param3 = in.param3;
866 : // out.param4 = in.param4;
867 : // out.x = 0; // we don't handle positioning when doing takeoffs
868 : // out.y = 0;
869 24 : out.z = -in.param7; // up -> down
870 24 : }
871 :
872 428567 : void GCS_MAVLINK_Plane::convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame)
873 : {
874 428567 : switch (in.command) {
875 24 : case MAV_CMD_NAV_TAKEOFF:
876 24 : convert_MAV_CMD_NAV_TAKEOFF_to_COMMAND_INT(in, out);
877 24 : return;
878 : }
879 428543 : return GCS_MAVLINK::convert_COMMAND_LONG_to_COMMAND_INT(in, out, frame);
880 : }
881 : #endif // AP_MAVLINK_COMMAND_LONG_ENABLED
882 :
883 12 : MAV_RESULT GCS_MAVLINK_Plane::handle_command_MAV_CMD_NAV_TAKEOFF(const mavlink_command_int_t &packet)
884 : {
885 12 : float takeoff_alt = packet.z;
886 12 : switch (packet.frame) {
887 12 : case MAV_FRAME_LOCAL_OFFSET_NED: // "NED local tangent frame with origin that travels with the vehicle"
888 12 : takeoff_alt = -takeoff_alt; // down -> up
889 12 : break;
890 0 : default:
891 0 : return MAV_RESULT_DENIED; // "is supported but has invalid parameters"
892 : }
893 12 : if (!plane.quadplane.available()) {
894 0 : return MAV_RESULT_FAILED;
895 : }
896 12 : if (!plane.quadplane.do_user_takeoff(takeoff_alt)) {
897 0 : return MAV_RESULT_FAILED;
898 : }
899 12 : return MAV_RESULT_ACCEPTED;
900 : }
901 : #endif
902 :
903 2 : MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_AUTOTUNE_ENABLE(const mavlink_command_int_t &packet)
904 : {
905 : // param1 : enable/disable
906 2 : plane.autotune_enable(!is_zero(packet.param1));
907 2 : return MAV_RESULT_ACCEPTED;
908 : }
909 :
910 : #if HAL_PARACHUTE_ENABLED
911 2 : MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet)
912 : {
913 : // configure or release parachute
914 2 : switch ((uint16_t)packet.param1) {
915 0 : case PARACHUTE_DISABLE:
916 0 : plane.parachute.enabled(false);
917 0 : return MAV_RESULT_ACCEPTED;
918 0 : case PARACHUTE_ENABLE:
919 0 : plane.parachute.enabled(true);
920 0 : return MAV_RESULT_ACCEPTED;
921 2 : case PARACHUTE_RELEASE:
922 : // treat as a manual release which performs some additional check of altitude
923 2 : if (plane.parachute.released()) {
924 0 : gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute already released");
925 0 : return MAV_RESULT_FAILED;
926 : }
927 2 : if (!plane.parachute.enabled()) {
928 0 : gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute not enabled");
929 0 : return MAV_RESULT_FAILED;
930 : }
931 2 : if (!plane.parachute_manual_release()) {
932 0 : return MAV_RESULT_FAILED;
933 : }
934 2 : return MAV_RESULT_ACCEPTED;
935 0 : default:
936 0 : break;
937 : }
938 0 : return MAV_RESULT_FAILED;
939 : }
940 : #endif
941 :
942 :
943 : #if HAL_QUADPLANE_ENABLED
944 4 : MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet)
945 : {
946 : // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
947 : // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
948 : // param3 : throttle (range depends upon param2)
949 : // param4 : timeout (in seconds)
950 : // param5 : motor count (number of motors to test in sequence)
951 4 : return plane.quadplane.mavlink_motor_test_start(chan,
952 4 : (uint8_t)packet.param1,
953 4 : (uint8_t)packet.param2,
954 4 : (uint16_t)packet.param3,
955 4 : packet.param4,
956 4 : (uint8_t)packet.x);
957 : }
958 :
959 4 : MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet)
960 : {
961 4 : if (!plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)packet.param1)) {
962 0 : return MAV_RESULT_FAILED;
963 : }
964 4 : return MAV_RESULT_ACCEPTED;
965 : }
966 : #endif
967 :
968 : // this is called on receipt of a MANUAL_CONTROL packet and is
969 : // expected to call manual_override to override RC input on desired
970 : // axes.
971 8 : void GCS_MAVLINK_Plane::handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow)
972 : {
973 8 : manual_override(plane.channel_roll, packet.y, 1000, 2000, tnow);
974 8 : manual_override(plane.channel_pitch, packet.x, 1000, 2000, tnow, true);
975 8 : manual_override(plane.channel_throttle, packet.z, 0, 1000, tnow);
976 8 : manual_override(plane.channel_rudder, packet.r, 1000, 2000, tnow);
977 8 : }
978 :
979 1145828 : void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
980 : {
981 1145828 : switch (msg.msgid) {
982 :
983 1041 : case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
984 1041 : handle_set_attitude_target(msg);
985 1041 : break;
986 :
987 0 : case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
988 0 : handle_set_position_target_local_ned(msg);
989 0 : break;
990 :
991 2 : case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
992 2 : handle_set_position_target_global_int(msg);
993 2 : break;
994 :
995 1144785 : default:
996 1144785 : GCS_MAVLINK::handle_message(msg);
997 1144286 : break;
998 : } // end switch
999 1145329 : } // end handle mavlink
1000 :
1001 1041 : void GCS_MAVLINK_Plane::handle_set_attitude_target(const mavlink_message_t &msg)
1002 : {
1003 : // Only allow companion computer (or other external controller) to
1004 : // control attitude in GUIDED mode. We DON'T want external control
1005 : // in e.g., RTL, CICLE. Specifying a single mode for companion
1006 : // computer control is more safe (even more so when using
1007 : // FENCE_ACTION = 4 for geofence failures).
1008 1041 : if (plane.control_mode != &plane.mode_guided) { // don't screw up failsafes
1009 0 : return;
1010 : }
1011 :
1012 : mavlink_set_attitude_target_t att_target;
1013 1041 : mavlink_msg_set_attitude_target_decode(&msg, &att_target);
1014 :
1015 : // Mappings: If any of these bits are set, the corresponding input should be ignored.
1016 : // NOTE, when parsing the bits we invert them for easier interpretation but transport has them inverted
1017 : // bit 1: body roll rate
1018 : // bit 2: body pitch rate
1019 : // bit 3: body yaw rate
1020 : // bit 4: unknown
1021 : // bit 5: unknown
1022 : // bit 6: reserved
1023 : // bit 7: throttle
1024 : // bit 8: attitude
1025 :
1026 : // if not setting all Quaternion values, use _rate flags to indicate which fields.
1027 :
1028 : // Extract the Euler roll angle from the Quaternion.
1029 : Quaternion q(att_target.q[0], att_target.q[1],
1030 1041 : att_target.q[2], att_target.q[3]);
1031 :
1032 : // NOTE: att_target.type_mask is inverted for easier interpretation
1033 1041 : att_target.type_mask = att_target.type_mask ^ 0xFF;
1034 :
1035 1041 : uint8_t attitude_mask = att_target.type_mask & 0b10000111; // q plus rpy
1036 :
1037 1041 : uint32_t now = AP_HAL::millis();
1038 1041 : if ((attitude_mask & 0b10000001) || // partial, including roll
1039 : (attitude_mask == 0b10000000)) { // all angles
1040 1041 : plane.guided_state.forced_rpy_cd.x = degrees(q.get_euler_roll()) * 100.0f;
1041 :
1042 : // Update timer for external roll to the nav control
1043 1041 : plane.guided_state.last_forced_rpy_ms.x = now;
1044 : }
1045 :
1046 1041 : if ((attitude_mask & 0b10000010) || // partial, including pitch
1047 : (attitude_mask == 0b10000000)) { // all angles
1048 1041 : plane.guided_state.forced_rpy_cd.y = degrees(q.get_euler_pitch()) * 100.0f;
1049 :
1050 : // Update timer for external pitch to the nav control
1051 1041 : plane.guided_state.last_forced_rpy_ms.y = now;
1052 : }
1053 :
1054 1041 : if ((attitude_mask & 0b10000100) || // partial, including yaw
1055 : (attitude_mask == 0b10000000)) { // all angles
1056 1041 : plane.guided_state.forced_rpy_cd.z = degrees(q.get_euler_yaw()) * 100.0f;
1057 :
1058 : // Update timer for external yaw to the nav control
1059 1041 : plane.guided_state.last_forced_rpy_ms.z = now;
1060 : }
1061 1041 : if (att_target.type_mask & 0b01000000) { // throttle
1062 257 : plane.guided_state.forced_throttle = att_target.thrust * 100.0f;
1063 :
1064 : // Update timer for external throttle
1065 257 : plane.guided_state.last_forced_throttle_ms = now;
1066 : }
1067 : }
1068 :
1069 0 : void GCS_MAVLINK_Plane::handle_set_position_target_local_ned(const mavlink_message_t &msg)
1070 : {
1071 : // decode packet
1072 : mavlink_set_position_target_local_ned_t packet;
1073 0 : mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
1074 :
1075 : // exit if vehicle is not in Guided mode
1076 0 : if (plane.control_mode != &plane.mode_guided) {
1077 0 : return;
1078 : }
1079 :
1080 : // only local moves for now
1081 0 : if (packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) {
1082 0 : return;
1083 : }
1084 :
1085 : // just do altitude for now
1086 0 : plane.next_WP_loc.alt += -packet.z*100.0;
1087 0 : gcs().send_text(MAV_SEVERITY_INFO, "Change alt to %.1f",
1088 0 : (double)((plane.next_WP_loc.alt - plane.home.alt)*0.01));
1089 : }
1090 :
1091 2 : void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_message_t &msg)
1092 : {
1093 : // Only want to allow companion computer position control when
1094 : // in a certain mode to avoid inadvertently sending these
1095 : // kinds of commands when the autopilot is responding to problems
1096 : // in modes such as RTL, CIRCLE, etc. Specifying ONLY one mode
1097 : // for companion computer control is more safe (provided
1098 : // one uses the FENCE_ACTION = 4 (RTL) for geofence failures).
1099 2 : if (plane.control_mode != &plane.mode_guided) {
1100 : //don't screw up failsafes
1101 0 : return;
1102 : }
1103 :
1104 : mavlink_set_position_target_global_int_t pos_target;
1105 2 : mavlink_msg_set_position_target_global_int_decode(&msg, &pos_target);
1106 :
1107 : Location::AltFrame frame;
1108 2 : if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)pos_target.coordinate_frame, frame)) {
1109 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSITION_TARGET_GLOBAL_INT");
1110 : // Even though other parts of the command may be valid, reject the whole thing.
1111 0 : return;
1112 : }
1113 :
1114 : // Unexpectedly, the mask is expecting "ones" for dimensions that should
1115 : // be IGNORED rather than INCLUDED. See mavlink documentation of the
1116 : // SET_POSITION_TARGET_GLOBAL_INT message, type_mask field.
1117 2 : const bool alt_ignore = (pos_target.type_mask & POSITION_TARGET_TYPEMASK_Z_IGNORE);
1118 2 : if (!alt_ignore) {
1119 : Location loc {
1120 : 0, // lat
1121 : 0, // lng
1122 1 : int32_t(pos_target.alt * 100), // m -> cm
1123 : frame,
1124 1 : };
1125 1 : handle_change_alt_request(loc);
1126 : }
1127 : }
1128 :
1129 0 : MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet)
1130 : {
1131 0 : const MAV_RESULT result = GCS_MAVLINK::handle_command_do_set_mission_current(packet);
1132 0 : if (result != MAV_RESULT_ACCEPTED) {
1133 0 : return result;
1134 : }
1135 :
1136 : // if you change this you must change handle_mission_set_current
1137 0 : plane.auto_state.next_wp_crosstrack = false;
1138 0 : if (plane.control_mode == &plane.mode_auto && plane.mission.state() == AP_Mission::MISSION_STOPPED) {
1139 0 : plane.mission.resume();
1140 : }
1141 :
1142 0 : return result;
1143 : }
1144 :
1145 : #if AP_MAVLINK_MISSION_SET_CURRENT_ENABLED
1146 939 : void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg)
1147 : {
1148 : // if you change this you must change handle_command_do_set_mission_current
1149 939 : plane.auto_state.next_wp_crosstrack = false;
1150 939 : GCS_MAVLINK::handle_mission_set_current(mission, msg);
1151 939 : if (plane.control_mode == &plane.mode_auto && plane.mission.state() == AP_Mission::MISSION_STOPPED) {
1152 0 : plane.mission.resume();
1153 : }
1154 939 : }
1155 : #endif
1156 :
1157 9 : uint64_t GCS_MAVLINK_Plane::capabilities() const
1158 : {
1159 : return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
1160 : MAV_PROTOCOL_CAPABILITY_COMMAND_INT |
1161 : MAV_PROTOCOL_CAPABILITY_MISSION_INT |
1162 : MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
1163 9 : MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
1164 : #if AP_TERRAIN_AVAILABLE
1165 9 : (plane.terrain.enabled() ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0) |
1166 : #endif
1167 9 : GCS_MAVLINK::capabilities());
1168 : }
1169 :
1170 : #if HAL_HIGH_LATENCY2_ENABLED
1171 729 : int16_t GCS_MAVLINK_Plane::high_latency_target_altitude() const
1172 : {
1173 729 : AP_AHRS &ahrs = AP::ahrs();
1174 729 : Location global_position_current;
1175 729 : UNUSED_RESULT(ahrs.get_location(global_position_current));
1176 :
1177 : #if HAL_QUADPLANE_ENABLED
1178 729 : const QuadPlane &quadplane = plane.quadplane;
1179 : //return units are m
1180 729 : if (quadplane.show_vtol_view()) {
1181 0 : return (plane.control_mode != &plane.mode_qstabilize) ? (global_position_current.alt * 0.01 - quadplane.pos_control->get_pos_error_D_m()) : 0;
1182 : }
1183 : #endif
1184 729 : return 0.01 * (global_position_current.alt + plane.calc_altitude_error_cm());
1185 : }
1186 :
1187 729 : uint8_t GCS_MAVLINK_Plane::high_latency_tgt_heading() const
1188 : {
1189 : // return units are deg/2
1190 : #if HAL_QUADPLANE_ENABLED
1191 729 : const QuadPlane &quadplane = plane.quadplane;
1192 729 : if (quadplane.show_vtol_view()) {
1193 0 : const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
1194 0 : return ((uint16_t)(targets.z * 0.01)) / 2;
1195 : }
1196 : #endif
1197 729 : const AP_Navigation *nav_controller = plane.nav_controller;
1198 : // need to convert -18000->18000 to 0->360/2
1199 729 : return wrap_360_cd(nav_controller->target_bearing_cd() ) / 200;
1200 : }
1201 :
1202 729 : uint16_t GCS_MAVLINK_Plane::high_latency_tgt_dist_dam() const
1203 : {
1204 : #if HAL_QUADPLANE_ENABLED
1205 729 : const QuadPlane &quadplane = plane.quadplane;
1206 729 : if (quadplane.show_vtol_view()) {
1207 0 : bool wp_nav_valid = quadplane.using_wp_nav();
1208 0 : return (wp_nav_valid ? MIN(static_cast<uint16_t>(quadplane.wp_nav->get_wp_distance_to_destination_cm() * 1e-3), UINT16_MAX) : 0);
1209 : }
1210 : #endif
1211 :
1212 729 : return MIN(static_cast<uint16_t>(plane.auto_state.wp_distance * 0.1), UINT16_MAX);
1213 : }
1214 :
1215 729 : uint8_t GCS_MAVLINK_Plane::high_latency_tgt_airspeed() const
1216 : {
1217 : // return units are m/s*5
1218 729 : return plane.target_airspeed_cm * 0.05;
1219 : }
1220 :
1221 729 : uint8_t GCS_MAVLINK_Plane::high_latency_wind_speed() const
1222 : {
1223 729 : Vector3f wind;
1224 : // use the estimate even if it is not marked valid, to preserve
1225 : // existing behaviour
1226 729 : IGNORE_RETURN(AP::ahrs().get_wind(wind));
1227 :
1228 : // return units are m/s*5
1229 729 : return MIN(wind.xy().length() * 5, UINT8_MAX);
1230 : }
1231 :
1232 729 : uint8_t GCS_MAVLINK_Plane::high_latency_wind_direction() const
1233 : {
1234 729 : Vector3f wind;
1235 : // use the estimate even if it is not marked valid, to preserve
1236 : // existing behaviour
1237 729 : IGNORE_RETURN(AP::ahrs().get_wind(wind));
1238 :
1239 : // return units are deg/2
1240 : // need to convert -180->180 to 0->360/2
1241 729 : return wrap_360(degrees(atan2f(-wind.y, -wind.x))) / 2;
1242 : }
1243 : #endif // HAL_HIGH_LATENCY2_ENABLED
1244 :
1245 3428 : MAV_VTOL_STATE GCS_MAVLINK_Plane::vtol_state() const
1246 : {
1247 : #if !HAL_QUADPLANE_ENABLED
1248 : return MAV_VTOL_STATE_UNDEFINED;
1249 : #else
1250 3428 : if (!plane.quadplane.available()) {
1251 0 : return MAV_VTOL_STATE_UNDEFINED;
1252 : }
1253 :
1254 3428 : return plane.quadplane.transition->get_mav_vtol_state();
1255 : #endif
1256 : };
1257 :
1258 3432 : MAV_LANDED_STATE GCS_MAVLINK_Plane::landed_state() const
1259 : {
1260 3432 : if (plane.is_flying()) {
1261 2885 : if (plane.is_taking_off()) {
1262 413 : return MAV_LANDED_STATE_TAKEOFF;
1263 : }
1264 2472 : if (plane.is_landing()) {
1265 528 : return MAV_LANDED_STATE_LANDING;
1266 : }
1267 :
1268 : // note that Q-modes almost always consider themselves as flying
1269 1944 : return MAV_LANDED_STATE_IN_AIR;
1270 : }
1271 :
1272 547 : return MAV_LANDED_STATE_ON_GROUND;
1273 : }
1274 :
1275 : // Send the mode with the given index (not mode number!) return the total number of modes
1276 : // Index starts at 1
1277 0 : uint8_t GCS_MAVLINK_Plane::send_available_mode(uint8_t index) const
1278 : {
1279 : // Fixed wing modes
1280 0 : const Mode* fw_modes[] {
1281 : &plane.mode_manual,
1282 : &plane.mode_circle,
1283 : &plane.mode_stabilize,
1284 : &plane.mode_training,
1285 : &plane.mode_acro,
1286 : &plane.mode_fbwa,
1287 : &plane.mode_fbwb,
1288 : &plane.mode_cruise,
1289 : &plane.mode_autotune,
1290 : &plane.mode_auto,
1291 : &plane.mode_rtl,
1292 : &plane.mode_loiter,
1293 : #if HAL_ADSB_ENABLED
1294 : &plane.mode_avoidADSB,
1295 : #endif
1296 : &plane.mode_guided,
1297 : &plane.mode_initializing,
1298 : &plane.mode_takeoff,
1299 : #if HAL_SOARING_ENABLED
1300 : &plane.mode_thermal,
1301 : #endif
1302 : #if MODE_AUTOLAND_ENABLED
1303 : &plane.mode_autoland,
1304 : #endif
1305 : };
1306 :
1307 0 : const uint8_t fw_mode_count = ARRAY_SIZE(fw_modes);
1308 :
1309 : // Fixedwing modes are always present
1310 0 : uint8_t mode_count = fw_mode_count;
1311 :
1312 : #if HAL_QUADPLANE_ENABLED
1313 : // Quadplane modes
1314 0 : const Mode* q_modes[] {
1315 : &plane.mode_qstabilize,
1316 : &plane.mode_qhover,
1317 : &plane.mode_qloiter,
1318 : &plane.mode_qland,
1319 : &plane.mode_qrtl,
1320 : &plane.mode_qacro,
1321 : &plane.mode_loiter_qland,
1322 : #if QAUTOTUNE_ENABLED
1323 : &plane.mode_qautotune,
1324 : #endif
1325 : };
1326 :
1327 : // Quadplane modes must be enabled
1328 0 : if (plane.quadplane.available()) {
1329 0 : mode_count += ARRAY_SIZE(q_modes);
1330 : }
1331 : #endif // HAL_QUADPLANE_ENABLED
1332 :
1333 :
1334 : // Convert to zero indexed
1335 0 : const uint8_t index_zero = index - 1;
1336 0 : if (index_zero >= mode_count) {
1337 : // Mode does not exist!?
1338 0 : return mode_count;
1339 : }
1340 :
1341 : // Ask the mode for its name and number
1342 : const char* name;
1343 : Mode::Number mode_number;
1344 :
1345 0 : if (index_zero < fw_mode_count) {
1346 : // A fixedwing mode
1347 0 : name = fw_modes[index_zero]->name();
1348 0 : mode_number = fw_modes[index_zero]->mode_number();
1349 :
1350 : } else {
1351 : #if HAL_QUADPLANE_ENABLED
1352 : // A Quadplane mode
1353 0 : const uint8_t q_index = index_zero - fw_mode_count;
1354 0 : name = q_modes[q_index]->name();
1355 0 : mode_number = q_modes[q_index]->mode_number();
1356 : #else
1357 : // Should not endup here
1358 : return mode_count;
1359 : #endif
1360 : }
1361 :
1362 : // All modes can be selected except Initialising and those disabled by FLTMODE_GCSBLOCK param
1363 0 : const bool user_selectable = (mode_number != Mode::Number::INITIALISING) && plane.gcs_mode_enabled(mode_number);
1364 :
1365 0 : mavlink_msg_available_modes_send(
1366 0 : chan,
1367 : mode_count,
1368 : index,
1369 : MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
1370 : (uint8_t)mode_number,
1371 : // MAV_MODE_PROPERTY bitmask,
1372 : user_selectable ? 0 : MAV_MODE_PROPERTY_NOT_USER_SELECTABLE,
1373 : name
1374 : );
1375 :
1376 0 : return mode_count;
1377 : }
|
