PX4 Firmware
PX4 Autopilot Software http://px4.io
mavlink_mission.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file mavlink_mission.cpp
36  * MAVLink mission manager implementation.
37  *
38  * @author Lorenz Meier <lorenz@px4.io>
39  * @author Julian Oes <julian@px4.io>
40  * @author Anton Babushkin <anton@px4.io>
41  */
42 
43 #include "mavlink_mission.h"
44 #include "mavlink_main.h"
45 
46 #include <lib/ecl/geo/geo.h>
47 #include <systemlib/err.h>
48 #include <drivers/drv_hrt.h>
49 #include <px4_platform_common/defines.h>
50 #include <mathlib/mathlib.h>
51 #include <matrix/math.hpp>
52 #include <navigator/navigation.h>
53 #include <uORB/topics/mission.h>
55 
56 using matrix::wrap_2pi;
57 
60 uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 };
63 constexpr uint16_t MavlinkMissionManager::MAX_COUNT[];
66 
67 
68 #define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
69  ((_msg.target_component == mavlink_system.compid) || \
70  (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
71  (_msg.target_component == MAV_COMP_ID_ALL)))
72 
74  _mavlink(mavlink)
75 {
77 }
78 
79 void
81 {
82  if (!_dataman_init) {
83  _dataman_init = true;
84 
85  /* lock MISSION_STATE item */
86  int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
87 
88  if (dm_lock_ret != 0) {
89  PX4_ERR("DM_KEY_MISSION_STATE lock failed");
90  }
91 
92  mission_s mission_state;
93  int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
94 
95  /* unlock MISSION_STATE item */
96  if (dm_lock_ret == 0) {
98  }
99 
100  if (ret > 0) {
101  _dataman_id = (dm_item_t)mission_state.dataman_id;
102  _count[MAV_MISSION_TYPE_MISSION] = mission_state.count;
103  _current_seq = mission_state.current_seq;
104 
105  } else if (ret < 0) {
106  PX4_ERR("offboard mission init failed (%i)", ret);
107  }
108 
110 
112  }
113 
115 }
116 
117 int
119 {
120  mission_stats_entry_s stats;
121  // initialize fence points count
122  int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
123 
124  if (ret == sizeof(mission_stats_entry_s)) {
125  _count[MAV_MISSION_TYPE_FENCE] = stats.num_items;
127  }
128 
129  return ret;
130 }
131 
132 int
134 {
135  mission_stats_entry_s stats;
136  // initialize safe points count
137  int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
138 
139  if (ret == sizeof(mission_stats_entry_s)) {
140  _count[MAV_MISSION_TYPE_RALLY] = stats.num_items;
141  }
142 
143  return ret;
144 }
145 
146 /**
147  * Publish mission topic to notify navigator about changes.
148  */
149 int
150 MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq)
151 {
152  // We want to make sure the whole struct is initialized including padding before getting written by dataman.
153  mission_s mission {};
154  mission.timestamp = hrt_absolute_time();
155  mission.dataman_id = dataman_id;
156  mission.count = count;
157  mission.current_seq = seq;
158 
159  /* update mission state in dataman */
160 
161  /* lock MISSION_STATE item */
162  int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
163 
164  if (dm_lock_ret != 0) {
165  PX4_ERR("DM_KEY_MISSION_STATE lock failed");
166  }
167 
168  int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
169 
170  /* unlock MISSION_STATE item */
171  if (dm_lock_ret == 0) {
173  }
174 
175  if (res == sizeof(mission_s)) {
176  /* update active mission state */
177  _dataman_id = dataman_id;
178  _count[MAV_MISSION_TYPE_MISSION] = count;
179  _current_seq = seq;
181 
182  /* mission state saved successfully, publish offboard_mission topic */
184 
185  return PX4_OK;
186 
187  } else {
188  PX4_ERR("WPM: can't save mission state");
189 
191  _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD");
192  }
193 
194  return PX4_ERROR;
195  }
196 }
197 int
199 {
200  mission_stats_entry_s stats;
201  stats.num_items = count;
202  stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data
203 
204  /* update stats in dataman */
206 
207  if (res == sizeof(mission_stats_entry_s)) {
208  _count[MAV_MISSION_TYPE_FENCE] = count;
209 
210  } else {
211 
213  _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD");
214  }
215 
216  return PX4_ERROR;
217  }
218 
219  return PX4_OK;
220 
221 }
222 
223 int
225 {
226  mission_stats_entry_s stats;
227  stats.num_items = count;
229 
230  /* update stats in dataman */
232 
233  if (res == sizeof(mission_stats_entry_s)) {
234  _count[MAV_MISSION_TYPE_RALLY] = count;
235 
236  } else {
237 
239  _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD");
240  }
241 
242  return PX4_ERROR;
243  }
244 
245  return PX4_OK;
246 }
247 
248 void
249 MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
250 {
251  mavlink_mission_ack_t wpa;
252 
253  wpa.target_system = sysid;
254  wpa.target_component = compid;
255  wpa.type = type;
256  wpa.mission_type = _mission_type;
257 
258  mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa);
259 
260  PX4_DEBUG("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system);
261 }
262 
263 void
265 {
266  unsigned item_count = _count[MAV_MISSION_TYPE_MISSION];
267 
268  if (seq < item_count) {
269  mavlink_mission_current_t wpc;
270 
271  wpc.seq = seq;
272 
273  mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc);
274 
275  } else if (seq == 0 && item_count == 0) {
276  /* don't broadcast if no WPs */
277 
278  } else {
279  PX4_DEBUG("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq);
280 
281  _mavlink->send_statustext_critical("ERROR: wp index out of bounds");
282  }
283 }
284 
285 
286 void
287 MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type)
288 {
290 
291  mavlink_mission_count_t wpc;
292 
293  wpc.target_system = sysid;
294  wpc.target_component = compid;
295  wpc.count = count;
296  wpc.mission_type = mission_type;
297 
298  mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc);
299 
300  PX4_DEBUG("WPM: Send MISSION_COUNT %u to ID %u, mission type=%i", wpc.count, wpc.target_system, mission_type);
301 }
302 
303 
304 void
305 MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
306 {
307  mission_item_s mission_item = {};
308  int read_result = 0;
309 
310  switch (_mission_type) {
311 
312  case MAV_MISSION_TYPE_MISSION: {
313  read_result = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s);
314  }
315  break;
316 
317  case MAV_MISSION_TYPE_FENCE: { // Read a geofence point
318  mission_fence_point_s mission_fence_point;
319  read_result = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)) ==
320  sizeof(mission_fence_point_s);
321 
322  mission_item.nav_cmd = mission_fence_point.nav_cmd;
323  mission_item.frame = mission_fence_point.frame;
324  mission_item.lat = mission_fence_point.lat;
325  mission_item.lon = mission_fence_point.lon;
326  mission_item.altitude = mission_fence_point.alt;
327 
328  if (mission_fence_point.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION ||
329  mission_fence_point.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
330  mission_item.vertex_count = mission_fence_point.vertex_count;
331 
332  } else {
333  mission_item.circle_radius = mission_fence_point.circle_radius;
334  }
335  }
336  break;
337 
338  case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
339  mission_safe_point_s mission_safe_point;
340  read_result = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s)) ==
341  sizeof(mission_safe_point_s);
342 
343  mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT;
344  mission_item.frame = mission_safe_point.frame;
345  mission_item.lat = mission_safe_point.lat;
346  mission_item.lon = mission_safe_point.lon;
347  mission_item.altitude = mission_safe_point.alt;
348  }
349  break;
350 
351  default:
352  _mavlink->send_statustext_critical("Received unknown mission type, abort.");
353  break;
354  }
355 
356  if (read_result > 0) {
358 
359  if (_int_mode) {
360  mavlink_mission_item_int_t wp = {};
361  format_mavlink_mission_item(&mission_item, reinterpret_cast<mavlink_mission_item_t *>(&wp));
362 
363  wp.target_system = sysid;
364  wp.target_component = compid;
365  wp.seq = seq;
366  wp.current = (_current_seq == seq) ? 1 : 0;
367 
368  mavlink_msg_mission_item_int_send_struct(_mavlink->get_channel(), &wp);
369 
370  PX4_DEBUG("WPM: Send MISSION_ITEM_INT seq %u to ID %u", wp.seq, wp.target_system);
371 
372  } else {
373  mavlink_mission_item_t wp = {};
374  format_mavlink_mission_item(&mission_item, &wp);
375 
376  wp.target_system = sysid;
377  wp.target_component = compid;
378  wp.seq = seq;
379  wp.current = (_current_seq == seq) ? 1 : 0;
380 
381  mavlink_msg_mission_item_send_struct(_mavlink->get_channel(), &wp);
382 
383  PX4_DEBUG("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system);
384  }
385 
386  } else {
388 
390  _mavlink->send_statustext_critical("Mission storage: Unable to read from microSD");
391  }
392 
393  PX4_DEBUG("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id);
394  }
395 }
396 
397 uint16_t
399 {
400  if (_mission_type >= sizeof(MAX_COUNT) / sizeof(MAX_COUNT[0])) {
401  PX4_ERR("WPM: MAX_COUNT out of bounds (%u)", _mission_type);
402  return 0;
403  }
404 
405  return MAX_COUNT[_mission_type];
406 }
407 
408 uint16_t
410 {
411  if (_mission_type >= sizeof(_count) / sizeof(_count[0])) {
412  PX4_ERR("WPM: _count out of bounds (%u)", _mission_type);
413  return 0;
414  }
415 
416  return _count[_mission_type];
417 }
418 
419 void
420 MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq)
421 {
422  if (seq < current_max_item_count()) {
424 
425  if (_int_mode) {
426  mavlink_mission_request_int_t wpr;
427  wpr.target_system = sysid;
428  wpr.target_component = compid;
429  wpr.seq = seq;
430  wpr.mission_type = _mission_type;
431  mavlink_msg_mission_request_int_send_struct(_mavlink->get_channel(), &wpr);
432 
433  PX4_DEBUG("WPM: Send MISSION_REQUEST_INT seq %u to ID %u", wpr.seq, wpr.target_system);
434 
435  } else {
436 
437  mavlink_mission_request_t wpr;
438  wpr.target_system = sysid;
439  wpr.target_component = compid;
440  wpr.seq = seq;
441  wpr.mission_type = _mission_type;
442 
443  mavlink_msg_mission_request_send_struct(_mavlink->get_channel(), &wpr);
444 
445  PX4_DEBUG("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system);
446  }
447 
448  } else {
449  _mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity");
450 
451  PX4_DEBUG("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq);
452  }
453 }
454 
455 
456 void
458 {
459  mavlink_mission_item_reached_t wp_reached;
460 
461  wp_reached.seq = seq;
462 
463  mavlink_msg_mission_item_reached_send_struct(_mavlink->get_channel(), &wp_reached);
464 
465  PX4_DEBUG("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq);
466 }
467 
468 
469 void
471 {
472  // do not send anything over high latency communication
474  return;
475  }
476 
477  mission_result_s mission_result{};
478 
479  if (_mission_result_sub.update(&mission_result)) {
480 
481  if (_current_seq != mission_result.seq_current) {
482  _current_seq = mission_result.seq_current;
483 
484  PX4_DEBUG("WPM: got mission result, new current_seq: %u", _current_seq);
485  }
486 
487  if (_last_reached != mission_result.seq_reached) {
488  _last_reached = mission_result.seq_reached;
490 
491  if (_last_reached >= 0) {
492  send_mission_item_reached((uint16_t)mission_result.seq_reached);
493  }
494 
495  PX4_DEBUG("WPM: got mission result, new seq_reached: %d", _last_reached);
496  }
497 
499 
500  if (mission_result.item_do_jump_changed) {
501  /* Send a mission item again if the remaining DO_JUMPs has changed, but don't interfere
502  * if there are ongoing transfers happening already. */
503  if (_state == MAVLINK_WPM_STATE_IDLE) {
504  _mission_type = MAV_MISSION_TYPE_MISSION;
506  (uint16_t)mission_result.item_changed_index);
507  }
508  }
509 
510  } else {
511  if (_slow_rate_limiter.check(now)) {
513 
514  // send the reached message another 10 times
515  if (_last_reached >= 0 && (_reached_sent_count < 10)) {
518  }
519  }
520  }
521 
522  /* check for timed-out operations */
523  if (_state == MAVLINK_WPM_STATE_GETLIST && (_time_last_sent > 0)
525 
526  // try to request item again after timeout
528 
529  } else if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0)
531 
532  _mavlink->send_statustext_critical("Operation timeout");
533 
534  PX4_DEBUG("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state);
535 
537 
538  // since we are giving up, reset this state also, so another request can be started.
539  _transfer_in_progress = false;
540 
541  } else if (_state == MAVLINK_WPM_STATE_IDLE) {
542  // reset flags
543  _time_last_sent = 0;
544  _time_last_recv = 0;
545  }
546 }
547 
548 
549 void
551 {
552  switch (msg->msgid) {
553  case MAVLINK_MSG_ID_MISSION_ACK:
554  handle_mission_ack(msg);
555  break;
556 
557  case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
559  break;
560 
561  case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
563  break;
564 
565  case MAVLINK_MSG_ID_MISSION_REQUEST:
567  break;
568 
569  case MAVLINK_MSG_ID_MISSION_REQUEST_INT:
571  break;
572 
573  case MAVLINK_MSG_ID_MISSION_COUNT:
575  break;
576 
577  case MAVLINK_MSG_ID_MISSION_ITEM:
578  handle_mission_item(msg);
579  break;
580 
581  case MAVLINK_MSG_ID_MISSION_ITEM_INT:
583  break;
584 
585  case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
587  break;
588 
589  default:
590  break;
591  }
592 }
593 
594 
595 void
597 {
598  mavlink_mission_ack_t wpa;
599  mavlink_msg_mission_ack_decode(msg, &wpa);
600 
601  if (CHECK_SYSID_COMPID_MISSION(wpa)) {
602  if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
603  if (_state == MAVLINK_WPM_STATE_SENDLIST && _mission_type == wpa.mission_type) {
605 
607  PX4_DEBUG("WPM: MISSION_ACK OK all items sent, switch to state IDLE");
608 
609  } else {
610  _mavlink->send_statustext_critical("WPM: ERR: not all items sent -> IDLE");
611 
612  PX4_DEBUG("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway");
613  }
614 
616 
617  } else if (_state == MAVLINK_WPM_STATE_GETLIST) {
618 
619  // INT or float mode is not supported
620  if (wpa.type == MAV_MISSION_UNSUPPORTED) {
621 
622  if (_int_mode) {
623  _int_mode = false;
625 
626  } else {
627  _int_mode = true;
629  }
630 
631  } else if (wpa.type != MAV_MISSION_ACCEPTED) {
632  PX4_WARN("Mission ack result was %d", wpa.type);
633  }
634  }
635 
636  } else {
637  _mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch");
638 
639  PX4_DEBUG("WPM: MISSION_ACK ERR: ID mismatch");
640  }
641  }
642 }
643 
644 
645 void
647 {
648  mavlink_mission_set_current_t wpc;
649  mavlink_msg_mission_set_current_decode(msg, &wpc);
650 
651  if (CHECK_SYSID_COMPID_MISSION(wpc)) {
652  if (_state == MAVLINK_WPM_STATE_IDLE) {
654 
655  if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) {
656  if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) {
657  PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq);
658 
659  } else {
660  PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq);
661 
662  _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID");
663  }
664 
665  } else {
666  PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq);
667 
668  _mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list");
669  }
670 
671  } else {
672  PX4_DEBUG("WPM: MISSION_SET_CURRENT ERROR: busy");
673 
674  _mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy");
675  }
676  }
677 }
678 
679 
680 void
682 {
683  mavlink_mission_request_list_t wprl;
684  mavlink_msg_mission_request_list_decode(msg, &wprl);
685 
686  if (CHECK_SYSID_COMPID_MISSION(wprl)) {
687  if (_state == MAVLINK_WPM_STATE_IDLE || (_state == MAVLINK_WPM_STATE_SENDLIST
688  && (uint8_t)_mission_type == wprl.mission_type)) {
690 
692  _mission_type = (MAV_MISSION_TYPE)wprl.mission_type;
693 
694  // make sure our item counts are up-to-date
695  switch (_mission_type) {
696  case MAV_MISSION_TYPE_FENCE:
698  break;
699 
700  case MAV_MISSION_TYPE_RALLY:
702  break;
703 
704  default:
705  break;
706  }
707 
708  _transfer_seq = 0;
710  _transfer_partner_sysid = msg->sysid;
711  _transfer_partner_compid = msg->compid;
712 
713  if (_transfer_count > 0) {
714  PX4_DEBUG("WPM: MISSION_REQUEST_LIST OK, %u mission items to send, mission type=%i", _transfer_count, _mission_type);
715 
716  } else {
717  PX4_DEBUG("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty, mission type=%i", _mission_type);
718  }
719 
720  send_mission_count(msg->sysid, msg->compid, _transfer_count, _mission_type);
721 
722  } else {
723  PX4_DEBUG("WPM: MISSION_REQUEST_LIST ERROR: busy");
724 
725  _mavlink->send_statustext_critical("IGN REQUEST LIST: Busy");
726  }
727  }
728 }
729 
730 
731 void
733 {
734  // The request comes in the old float mode, so we switch to it.
735  if (_int_mode) {
736  _int_mode = false;
737  }
738 
740 }
741 
742 void
744 {
745  // The request comes in the new int mode, so we switch to it.
746  if (!_int_mode) {
747  _int_mode = true;
748  }
749 
751 }
752 
753 void
755 {
756  /* The mavlink_message_t could also be a mavlink_mission_request_int_t, however the structs
757  * are basically the same, so we can ignore it. */
758  mavlink_mission_request_t wpr;
759  mavlink_msg_mission_request_decode(msg, &wpr);
760 
761  if (CHECK_SYSID_COMPID_MISSION(wpr)) {
762  if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
763  if (_state == MAVLINK_WPM_STATE_SENDLIST) {
764 
765  if (_mission_type != wpr.mission_type) {
766  PX4_WARN("WPM: Unexpected mission type (%u %u)", wpr.mission_type, _mission_type);
767  return;
768  }
769 
771 
772  /* _transfer_seq contains sequence of expected request */
773  if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) {
774  PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) seq %u from ID %u", wpr.seq, msg->sysid);
775 
776  _transfer_seq++;
777 
778  } else if (wpr.seq == _transfer_seq - 1) {
779  PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) seq %u from ID %u (again)", wpr.seq, msg->sysid);
780 
781  } else {
783  PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid,
785 
786  } else if (_transfer_seq <= 0) {
787  PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid,
788  _transfer_seq);
789 
790  } else {
791  PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid,
792  _transfer_seq - 1);
793  }
794 
796 
798  _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
799  return;
800  }
801 
802  /* double check bounds in case of items count changed */
803  if (wpr.seq < current_item_count()) {
805 
806  } else {
807  PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", wpr.seq, wpr.seq,
808  current_item_count() - 1);
809 
811 
813  _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
814  }
815 
816  } else if (_state == MAVLINK_WPM_STATE_IDLE) {
817  PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: no transfer");
818 
819  // Silently ignore this as some OSDs have buggy mission protocol implementations
820  //_mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST(_INT): No active transfer");
821 
822  } else {
823  PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: busy (state %d).", _state);
824 
825  _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy");
826  }
827 
828  } else {
829  _mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch");
830 
831  PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: rejected, partner ID mismatch");
832  }
833  }
834 }
835 
836 
837 void
839 {
840  mavlink_mission_count_t wpc;
841  mavlink_msg_mission_count_decode(msg, &wpc);
842 
843  if (CHECK_SYSID_COMPID_MISSION(wpc)) {
844  if (_state == MAVLINK_WPM_STATE_IDLE) {
846 
847  if (_transfer_in_progress) {
849  return;
850  }
851 
852  _transfer_in_progress = true;
853  _mission_type = (MAV_MISSION_TYPE)wpc.mission_type;
854 
855  if (wpc.count > current_max_item_count()) {
856  PX4_DEBUG("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, current_max_item_count());
857 
859  _transfer_in_progress = false;
860  return;
861  }
862 
863  if (wpc.count == 0) {
864  PX4_DEBUG("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE");
865 
866  switch (_mission_type) {
867  case MAV_MISSION_TYPE_MISSION:
868 
869  /* alternate dataman ID anyway to let navigator know about changes */
870 
873 
874  } else {
876  }
877 
878  break;
879 
880  case MAV_MISSION_TYPE_FENCE:
882  break;
883 
884  case MAV_MISSION_TYPE_RALLY:
886  break;
887 
888  default:
889  PX4_ERR("mission type %u not handled", _mission_type);
890  break;
891  }
892 
894  _transfer_in_progress = false;
895  return;
896  }
897 
898  PX4_DEBUG("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
899 
900  _state = MAVLINK_WPM_STATE_GETLIST;
901  _transfer_seq = 0;
902  _transfer_partner_sysid = msg->sysid;
903  _transfer_partner_compid = msg->compid;
904  _transfer_count = wpc.count;
906  DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission
908 
909  if (_mission_type == MAV_MISSION_TYPE_FENCE) {
910  // We're about to write new geofence items, so take the lock. It will be released when
911  // switching back to idle
912  PX4_DEBUG("locking fence dataman items");
913 
914  int ret = dm_lock(DM_KEY_FENCE_POINTS);
915 
916  if (ret == 0) {
917  _geofence_locked = true;
918 
919  } else {
920  PX4_ERR("locking failed (%i)", errno);
921  }
922  }
923 
924  } else if (_state == MAVLINK_WPM_STATE_GETLIST) {
926 
927  if (_transfer_seq == 0) {
928  /* looks like our MISSION_REQUEST was lost, try again */
929  PX4_DEBUG("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid);
930 
931  } else {
932  PX4_DEBUG("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq);
933 
934  _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy");
935 
937  return;
938  }
939 
940  } else {
941  PX4_DEBUG("WPM: MISSION_COUNT ERROR: busy, state %i", _state);
942 
943  _mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy");
945  return;
946  }
947 
949  }
950 }
951 
952 void
954 {
955  // when switching to idle, we *always* check if the lock was held and release it.
956  // This is to ensure we don't end up in a state where we forget to release it.
957  if (_geofence_locked) {
959  _geofence_locked = false;
960 
961  PX4_DEBUG("unlocking geofence");
962  }
963 
964  _state = MAVLINK_WPM_STATE_IDLE;
965 }
966 
967 
968 void
970 {
971  if (_int_mode) {
972  // It seems that we should be using the float mode, let's switch out of int mode.
973  _int_mode = false;
974  }
975 
977 }
978 
979 void
981 {
982  if (!_int_mode) {
983  // It seems that we should be using the int mode, let's switch to it.
984  _int_mode = true;
985  }
986 
988 }
989 
990 void
992 {
993 
994  // The mavlink_message could also contain a mavlink_mission_item_int_t. We ignore that here
995  // and take care of it later in parse_mavlink_mission_item depending on _int_mode.
996 
997  mavlink_mission_item_t wp;
998  mavlink_msg_mission_item_decode(msg, &wp);
999 
1000  if (CHECK_SYSID_COMPID_MISSION(wp)) {
1001 
1002  if (wp.mission_type != _mission_type) {
1003  PX4_WARN("WPM: Unexpected mission type (%u %u)", (int)wp.mission_type, (int)_mission_type);
1005  return;
1006  }
1007 
1008  if (_state == MAVLINK_WPM_STATE_GETLIST) {
1010 
1011  if (wp.seq != _transfer_seq) {
1012  PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq);
1013 
1014  /* request next item again */
1016  return;
1017  }
1018 
1019  } else if (_state == MAVLINK_WPM_STATE_IDLE) {
1020  if (_transfer_seq == wp.seq + 1) {
1021  // Assume this is a duplicate, where we already successfully got all mission items,
1022  // but the GCS did not receive the last ack and sent the same item again
1024 
1025  } else {
1026  PX4_DEBUG("WPM: MISSION_ITEM ERROR: no transfer");
1027 
1028  _mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer");
1030  }
1031 
1032  return;
1033 
1034  } else {
1035  PX4_DEBUG("WPM: MISSION_ITEM ERROR: busy, state %i", _state);
1036 
1037  _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
1039  return;
1040  }
1041 
1042  struct mission_item_s mission_item = {};
1043 
1044  int ret = parse_mavlink_mission_item(&wp, &mission_item);
1045 
1046  if (ret != PX4_OK) {
1047  PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq);
1048 
1049  _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
1050 
1053  _transfer_in_progress = false;
1054  return;
1055  }
1056 
1057  bool write_failed = false;
1058  bool check_failed = false;
1059 
1060  switch (_mission_type) {
1061 
1062  case MAV_MISSION_TYPE_MISSION: {
1063  // check that we don't get a wrong item (hardening against wrong client implementations, the list here
1064  // does not need to be complete)
1065  if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION ||
1066  mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION ||
1067  mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION ||
1068  mission_item.nav_cmd == MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION ||
1069  mission_item.nav_cmd == MAV_CMD_NAV_RALLY_POINT) {
1070  check_failed = true;
1071 
1072  } else {
1073  dm_item_t dm_item = _transfer_dataman_id;
1074 
1075  write_failed = dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item,
1076  sizeof(struct mission_item_s)) != sizeof(struct mission_item_s);
1077 
1078  if (!write_failed) {
1079  /* waypoint marked as current */
1080  if (wp.current) {
1081  _transfer_current_seq = wp.seq;
1082  }
1083  }
1084  }
1085  }
1086  break;
1087 
1088  case MAV_MISSION_TYPE_FENCE: { // Write a geofence point
1089  mission_fence_point_s mission_fence_point;
1090  mission_fence_point.nav_cmd = mission_item.nav_cmd;
1091  mission_fence_point.lat = mission_item.lat;
1092  mission_fence_point.lon = mission_item.lon;
1093  mission_fence_point.alt = mission_item.altitude;
1094 
1095  if (mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION ||
1096  mission_item.nav_cmd == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) {
1097  mission_fence_point.vertex_count = mission_item.vertex_count;
1098 
1099  if (mission_item.vertex_count < 3) { // feasibility check
1100  PX4_ERR("Fence: too few vertices");
1101  check_failed = true;
1103  }
1104 
1105  } else {
1106  mission_fence_point.circle_radius = mission_item.circle_radius;
1107  }
1108 
1109  mission_fence_point.frame = mission_item.frame;
1110 
1111  if (!check_failed) {
1112  write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_fence_point,
1113  sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s);
1114  }
1115 
1116  }
1117  break;
1118 
1119  case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point
1120  mission_safe_point_s mission_safe_point;
1121  mission_safe_point.lat = mission_item.lat;
1122  mission_safe_point.lon = mission_item.lon;
1123  mission_safe_point.alt = mission_item.altitude;
1124  mission_safe_point.frame = mission_item.frame;
1125  write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_safe_point,
1126  sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s);
1127  }
1128  break;
1129 
1130  default:
1131  _mavlink->send_statustext_critical("Received unknown mission type, abort.");
1132  break;
1133  }
1134 
1135  if (write_failed || check_failed) {
1136  PX4_DEBUG("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id);
1137 
1139 
1140  if (write_failed) {
1141  _mavlink->send_statustext_critical("Unable to write on micro SD");
1142  }
1143 
1145  _transfer_in_progress = false;
1146  return;
1147  }
1148 
1149  /* waypoint marked as current */
1150  if (wp.current) {
1151  _transfer_current_seq = wp.seq;
1152  }
1153 
1154  PX4_DEBUG("WPM: MISSION_ITEM seq %u received", wp.seq);
1155 
1156  _transfer_seq = wp.seq + 1;
1157 
1158  if (_transfer_seq == _transfer_count) {
1159  /* got all new mission items successfully */
1160  PX4_DEBUG("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE",
1162 
1163  ret = 0;
1164 
1165  switch (_mission_type) {
1166  case MAV_MISSION_TYPE_MISSION:
1168  break;
1169 
1170  case MAV_MISSION_TYPE_FENCE:
1172  break;
1173 
1174  case MAV_MISSION_TYPE_RALLY:
1176  break;
1177 
1178  default:
1179  PX4_ERR("mission type %u not handled", _mission_type);
1180  break;
1181  }
1182 
1183  // Note: the switch to idle needs to happen after update_geofence_count is called, for proper unlocking order
1185 
1186 
1187  if (ret == PX4_OK) {
1189 
1190  } else {
1192  }
1193 
1194  _transfer_in_progress = false;
1195 
1196  } else {
1197  /* request next item */
1199  }
1200  }
1201 }
1202 
1203 
1204 void
1206 {
1207  mavlink_mission_clear_all_t wpca;
1208  mavlink_msg_mission_clear_all_decode(msg, &wpca);
1209 
1210  if (CHECK_SYSID_COMPID_MISSION(wpca)) {
1211 
1212  if (_state == MAVLINK_WPM_STATE_IDLE) {
1213  /* don't touch mission items storage itself, but only items count in mission state */
1215 
1216  _mission_type = (MAV_MISSION_TYPE)wpca.mission_type; // this is needed for the returned ack
1217  int ret = 0;
1218 
1219  switch (wpca.mission_type) {
1220  case MAV_MISSION_TYPE_MISSION:
1223  break;
1224 
1225  case MAV_MISSION_TYPE_FENCE:
1226  ret = update_geofence_count(0);
1227  break;
1228 
1229  case MAV_MISSION_TYPE_RALLY:
1230  ret = update_safepoint_count(0);
1231  break;
1232 
1233  case MAV_MISSION_TYPE_ALL:
1234  ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
1235  DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
1236  ret = update_geofence_count(0) || ret;
1237  ret = update_safepoint_count(0) || ret;
1238  break;
1239 
1240  default:
1241  PX4_ERR("mission type %u not handled", _mission_type);
1242  break;
1243  }
1244 
1245  if (ret == PX4_OK) {
1246  PX4_DEBUG("WPM: CLEAR_ALL OK (mission_type=%i)", _mission_type);
1247 
1249 
1250  } else {
1252  }
1253 
1254  } else {
1255  _mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy");
1256 
1257  PX4_DEBUG("WPM: CLEAR_ALL IGNORED: busy");
1258  }
1259  }
1260 }
1261 
1262 int
1263 MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item,
1264  struct mission_item_s *mission_item)
1265 {
1266  if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
1267  mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT ||
1268  (_int_mode && (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
1269  mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT))) {
1270 
1271  // Switch to int mode if that is what we are receiving
1272  if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
1273  mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)) {
1274  _int_mode = true;
1275  }
1276 
1277  if (_int_mode) {
1278  /* The argument is actually a mavlink_mission_item_int_t in int_mode.
1279  * mavlink_mission_item_t and mavlink_mission_item_int_t have the same
1280  * alignment, so we can just swap float for int32_t. */
1281  const mavlink_mission_item_int_t *item_int
1282  = reinterpret_cast<const mavlink_mission_item_int_t *>(mavlink_mission_item);
1283  mission_item->lat = ((double)item_int->x) * 1e-7;
1284  mission_item->lon = ((double)item_int->y) * 1e-7;
1285 
1286  } else {
1287  mission_item->lat = (double)mavlink_mission_item->x;
1288  mission_item->lon = (double)mavlink_mission_item->y;
1289  }
1290 
1291  mission_item->altitude = mavlink_mission_item->z;
1292 
1293  if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL ||
1294  mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT) {
1295  mission_item->altitude_is_relative = false;
1296 
1297  } else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT ||
1298  mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
1299  mission_item->altitude_is_relative = true;
1300  }
1301 
1302  /* this field is shared with pitch_min (and circle_radius for geofence) in memory and
1303  * exclusive in the MAVLink spec. Set it to 0 first
1304  * and then set minimum pitch later only for the
1305  * corresponding item
1306  */
1307  mission_item->time_inside = 0.0f;
1308 
1309  switch (mavlink_mission_item->command) {
1310  case MAV_CMD_NAV_WAYPOINT:
1311  mission_item->nav_cmd = NAV_CMD_WAYPOINT;
1312  mission_item->time_inside = mavlink_mission_item->param1;
1313  mission_item->acceptance_radius = mavlink_mission_item->param2;
1314  mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
1315  break;
1316 
1317  case MAV_CMD_NAV_LOITER_UNLIM:
1318  mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
1319  mission_item->loiter_radius = mavlink_mission_item->param3;
1320  mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
1321  break;
1322 
1323  case MAV_CMD_NAV_LOITER_TIME:
1324  mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
1325  mission_item->time_inside = mavlink_mission_item->param1;
1326  mission_item->loiter_radius = mavlink_mission_item->param3;
1327  mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0);
1328  // Yaw is only valid for multicopter but we set it always because
1329  // it's just ignored for fixedwing.
1330  mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
1331  break;
1332 
1333  case MAV_CMD_NAV_LAND:
1334  mission_item->nav_cmd = NAV_CMD_LAND;
1335  // TODO: abort alt param1
1336  mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
1337  mission_item->land_precision = mavlink_mission_item->param2;
1338  break;
1339 
1340  case MAV_CMD_NAV_TAKEOFF:
1341  mission_item->nav_cmd = NAV_CMD_TAKEOFF;
1342  mission_item->pitch_min = mavlink_mission_item->param1;
1343  mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
1344  break;
1345 
1346  case MAV_CMD_NAV_LOITER_TO_ALT:
1347  mission_item->nav_cmd = NAV_CMD_LOITER_TO_ALT;
1348  mission_item->force_heading = (mavlink_mission_item->param1 > 0);
1349  mission_item->loiter_radius = mavlink_mission_item->param2;
1350  mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0);
1351  break;
1352 
1353  case MAV_CMD_NAV_ROI:
1354  case MAV_CMD_DO_SET_ROI:
1355  if ((int)mavlink_mission_item->param1 == MAV_ROI_LOCATION) {
1356  mission_item->nav_cmd = NAV_CMD_DO_SET_ROI;
1357  mission_item->params[0] = MAV_ROI_LOCATION;
1358 
1359  mission_item->params[6] = mavlink_mission_item->z;
1360 
1361  } else if ((int)mavlink_mission_item->param1 == MAV_ROI_NONE) {
1362  mission_item->nav_cmd = NAV_CMD_DO_SET_ROI;
1363  mission_item->params[0] = MAV_ROI_NONE;
1364 
1365  } else {
1366  return MAV_MISSION_INVALID_PARAM1;
1367  }
1368 
1369  break;
1370 
1371  case MAV_CMD_DO_SET_ROI_LOCATION:
1372  mission_item->nav_cmd = NAV_CMD_DO_SET_ROI_LOCATION;
1373  mission_item->params[6] = mavlink_mission_item->z;
1374  break;
1375 
1376  case MAV_CMD_NAV_VTOL_TAKEOFF:
1377  case MAV_CMD_NAV_VTOL_LAND:
1378  mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
1379  mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
1380  break;
1381 
1382  case MAV_CMD_NAV_FENCE_RETURN_POINT:
1383  mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
1384  break;
1385 
1386  case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
1387  case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
1388  mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
1389  mission_item->vertex_count = (uint16_t)(mavlink_mission_item->param1 + 0.5f);
1390  break;
1391 
1392  case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION:
1393  case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION:
1394  mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
1395  mission_item->circle_radius = mavlink_mission_item->param1;
1396  break;
1397 
1398  case MAV_CMD_NAV_RALLY_POINT:
1399  mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
1400  break;
1401 
1402  default:
1403  mission_item->nav_cmd = NAV_CMD_INVALID;
1404 
1405  PX4_DEBUG("Unsupported command %d", mavlink_mission_item->command);
1406 
1407  return MAV_MISSION_UNSUPPORTED;
1408  }
1409 
1410  mission_item->frame = mavlink_mission_item->frame;
1411 
1412  } else if (mavlink_mission_item->frame == MAV_FRAME_MISSION) {
1413 
1414  // this is a mission item with no coordinates
1415 
1416  mission_item->params[0] = mavlink_mission_item->param1;
1417  mission_item->params[1] = mavlink_mission_item->param2;
1418  mission_item->params[2] = mavlink_mission_item->param3;
1419  mission_item->params[3] = mavlink_mission_item->param4;
1420  mission_item->params[4] = mavlink_mission_item->x;
1421  mission_item->params[5] = mavlink_mission_item->y;
1422  mission_item->params[6] = mavlink_mission_item->z;
1423 
1424  switch (mavlink_mission_item->command) {
1425  case MAV_CMD_DO_JUMP:
1426  mission_item->nav_cmd = NAV_CMD_DO_JUMP;
1427  mission_item->do_jump_mission_index = mavlink_mission_item->param1;
1428  mission_item->do_jump_current_count = 0;
1429  mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
1430  break;
1431 
1432  case MAV_CMD_NAV_ROI:
1433  case MAV_CMD_DO_SET_ROI: {
1434  const int roi_mode = mavlink_mission_item->param1;
1435 
1436  if (roi_mode == MAV_ROI_NONE || roi_mode == MAV_ROI_WPNEXT || roi_mode == MAV_ROI_WPINDEX) {
1437  mission_item->nav_cmd = NAV_CMD_DO_SET_ROI;
1438 
1439  } else {
1440  return MAV_MISSION_INVALID_PARAM1;
1441  }
1442  }
1443  break;
1444 
1445  case MAV_CMD_DO_CHANGE_SPEED:
1446  case MAV_CMD_DO_SET_HOME:
1447  case MAV_CMD_DO_SET_SERVO:
1448  case MAV_CMD_DO_LAND_START:
1449  case MAV_CMD_DO_TRIGGER_CONTROL:
1450  case MAV_CMD_DO_DIGICAM_CONTROL:
1451  case MAV_CMD_DO_MOUNT_CONFIGURE:
1452  case MAV_CMD_DO_MOUNT_CONTROL:
1453  case MAV_CMD_IMAGE_START_CAPTURE:
1454  case MAV_CMD_IMAGE_STOP_CAPTURE:
1455  case MAV_CMD_VIDEO_START_CAPTURE:
1456  case MAV_CMD_VIDEO_STOP_CAPTURE:
1457  case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
1458  case MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL:
1459  case MAV_CMD_SET_CAMERA_MODE:
1460  case MAV_CMD_DO_VTOL_TRANSITION:
1461  case MAV_CMD_NAV_DELAY:
1462  case MAV_CMD_NAV_RETURN_TO_LAUNCH:
1463  case MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET:
1464  case MAV_CMD_DO_SET_ROI_NONE:
1465  mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
1466  break;
1467 
1468  default:
1469  mission_item->nav_cmd = NAV_CMD_INVALID;
1470 
1471  PX4_DEBUG("Unsupported command %d", mavlink_mission_item->command);
1472 
1473  return MAV_MISSION_UNSUPPORTED;
1474  }
1475 
1476  mission_item->frame = MAV_FRAME_MISSION;
1477 
1478  } else {
1479  PX4_DEBUG("Unsupported frame %d", mavlink_mission_item->frame);
1480 
1481  return MAV_MISSION_UNSUPPORTED_FRAME;
1482  }
1483 
1484  mission_item->autocontinue = mavlink_mission_item->autocontinue;
1485  // mission_item->index = mavlink_mission_item->seq;
1486 
1487  mission_item->origin = ORIGIN_MAVLINK;
1488 
1489  return MAV_MISSION_ACCEPTED;
1490 }
1491 
1492 
1493 int
1495  mavlink_mission_item_t *mavlink_mission_item)
1496 {
1497  mavlink_mission_item->frame = mission_item->frame;
1498  mavlink_mission_item->command = mission_item->nav_cmd;
1499  mavlink_mission_item->autocontinue = mission_item->autocontinue;
1500 
1501  /* default mappings for generic commands */
1502  if (mission_item->frame == MAV_FRAME_MISSION) {
1503  mavlink_mission_item->param1 = mission_item->params[0];
1504  mavlink_mission_item->param2 = mission_item->params[1];
1505  mavlink_mission_item->param3 = mission_item->params[2];
1506  mavlink_mission_item->param4 = mission_item->params[3];
1507  mavlink_mission_item->x = mission_item->params[4];
1508  mavlink_mission_item->y = mission_item->params[5];
1509  mavlink_mission_item->z = mission_item->params[6];
1510 
1511  switch (mavlink_mission_item->command) {
1512  case NAV_CMD_DO_JUMP:
1513  mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
1514  mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
1515  break;
1516 
1518  case NAV_CMD_DO_SET_HOME:
1519  case NAV_CMD_DO_SET_SERVO:
1520  case NAV_CMD_DO_LAND_START:
1529  case NAV_CMD_DO_SET_ROI:
1534  break;
1535 
1536  default:
1537  return PX4_ERROR;
1538  }
1539 
1540  } else {
1541  mavlink_mission_item->param1 = 0.0f;
1542  mavlink_mission_item->param2 = 0.0f;
1543  mavlink_mission_item->param3 = 0.0f;
1544  mavlink_mission_item->param4 = 0.0f;
1545 
1546  if (_int_mode) {
1547  // This function actually receives a mavlink_mission_item_int_t in _int_mode
1548  // which has the same alignment as mavlink_mission_item_t and the only
1549  // difference is int32_t vs. float for x and y.
1550  mavlink_mission_item_int_t *item_int =
1551  reinterpret_cast<mavlink_mission_item_int_t *>(mavlink_mission_item);
1552 
1553  item_int->x = round(mission_item->lat * 1e7);
1554  item_int->y = round(mission_item->lon * 1e7);
1555 
1556  } else {
1557  mavlink_mission_item->x = (float)mission_item->lat;
1558  mavlink_mission_item->y = (float)mission_item->lon;
1559  }
1560 
1561  mavlink_mission_item->z = mission_item->altitude;
1562 
1563  if (mission_item->altitude_is_relative) {
1564  if (_int_mode) {
1565  mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT;
1566 
1567  } else {
1568  mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
1569  }
1570 
1571  } else {
1572  if (_int_mode) {
1573  mavlink_mission_item->frame = MAV_FRAME_GLOBAL_INT;
1574 
1575  } else {
1576  mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
1577  }
1578  }
1579 
1580  switch (mission_item->nav_cmd) {
1581  case NAV_CMD_WAYPOINT:
1582  mavlink_mission_item->param1 = mission_item->time_inside;
1583  mavlink_mission_item->param2 = mission_item->acceptance_radius;
1584  mavlink_mission_item->param4 = math::degrees(mission_item->yaw);
1585  break;
1586 
1588  mavlink_mission_item->param3 = mission_item->loiter_radius;
1589  mavlink_mission_item->param4 = math::degrees(mission_item->yaw);
1590  break;
1591 
1593  mavlink_mission_item->param1 = mission_item->time_inside;
1594  mavlink_mission_item->param3 = mission_item->loiter_radius;
1595  mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack;
1596  break;
1597 
1598  case NAV_CMD_LAND:
1599  // TODO: param1 abort alt
1600  mavlink_mission_item->param2 = mission_item->land_precision;
1601  mavlink_mission_item->param4 = math::degrees(mission_item->yaw);
1602  break;
1603 
1604  case NAV_CMD_TAKEOFF:
1605  mavlink_mission_item->param1 = mission_item->pitch_min;
1606  mavlink_mission_item->param4 = math::degrees(mission_item->yaw);
1607  break;
1608 
1609  case NAV_CMD_LOITER_TO_ALT:
1610  mavlink_mission_item->param1 = mission_item->force_heading;
1611  mavlink_mission_item->param2 = mission_item->loiter_radius;
1612  mavlink_mission_item->param4 = mission_item->loiter_exit_xtrack;
1613  break;
1614 
1615  case MAV_CMD_NAV_VTOL_TAKEOFF:
1616  case MAV_CMD_NAV_VTOL_LAND:
1617  mavlink_mission_item->param4 = math::degrees(mission_item->yaw);
1618  break;
1619 
1620  case MAV_CMD_NAV_FENCE_RETURN_POINT:
1621  break;
1622 
1623  case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
1624  case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
1625  mavlink_mission_item->param1 = (float)mission_item->vertex_count;
1626  break;
1627 
1628  case MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION:
1629  case MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION:
1630  mavlink_mission_item->param1 = mission_item->circle_radius;
1631  break;
1632 
1633  case MAV_CMD_NAV_RALLY_POINT:
1634  break;
1635 
1636 
1637  default:
1638  return PX4_ERROR;
1639  }
1640  }
1641 
1642  return PX4_OK;
1643 }
1644 
1645 
1647 {
1648  // do not send anything over high latency communication
1650  return;
1651  }
1652 
1653  if (!(_my_dataman_id == _dataman_id)) {
1654  PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating");
1655 
1658  MAV_MISSION_TYPE_MISSION);
1659  }
1660 }
Global position setpoint in WGS84 coordinates.
Definition: navigation.h:145
uint16_t frame
mission frame
Definition: navigation.h:177
Definition of geo / math functions to perform geodesic calculations.
uint16_t vertex_count
Polygon vertex count (geofence)
Definition: navigation.h:172
float altitude
altitude in meters (AMSL)
Definition: navigation.h:160
uint16_t autocontinue
true if next waypoint should follow after this one
Definition: navigation.h:177
float acceptance_radius
default radius in which the mission is accepted as reached in meters
Definition: navigation.h:155
__EXPORT int dm_lock(dm_item_t item)
Lock all items of a type.
Definition: dataman.cpp:1157
uint64_t timestamp
Definition: mission.h:53
dm_item_t
Types of items that the data manager can store.
Definition: dataman.h:50
uint8_t dataman_id
Definition: mission.h:56
uint16_t origin
how the mission item was generated
Definition: navigation.h:177
uint16_t vertex_count
number of vertices in this polygon
Definition: navigation.h:209
uint16_t do_jump_repeat_count
how many times do jump needs to be done
Definition: navigation.h:168
constexpr T degrees(T radians)
Definition: Limits.hpp:91
float circle_radius
geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*)
Definition: navigation.h:210
High-resolution timer with callouts and timekeeping.
double lon
longitude in degrees
Definition: navigation.h:147
Type wrap_2pi(Type x)
Wrap value in range [0, 2π)
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
uint16_t count
Definition: mission.h:55
__EXPORT ssize_t dm_read(dm_item_t item, unsigned index, void *buf, size_t count)
Retrieve from the data manager file.
Definition: dataman.cpp:1104
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
Geofence vertex point.
Definition: navigation.h:203
float circle_radius
geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*)
Definition: navigation.h:153
uint16_t nav_cmd
navigation command
Definition: navigation.h:165
constexpr T radians(T degrees)
Definition: Limits.hpp:85
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
uint16_t do_jump_current_count
count how many times the jump has been done
Definition: navigation.h:171
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
dataman housekeeping information for a specific item.
Definition: navigation.h:194
float time_inside
time that the MAV should stay inside the radius before advancing in seconds
Definition: navigation.h:151
Safe Point (Rally Point).
Definition: navigation.h:223
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
float params[7]
array to store mission command values for MAV_FRAME_MISSION
Definition: navigation.h:162
__EXPORT void dm_unlock(dm_item_t item)
Unlock a data Item.
Definition: dataman.cpp:1202
uint16_t land_precision
Defines if landing should be precise: 0 = normal landing, 1 = opportunistic precision landing...
Definition: navigation.h:173
uint16_t nav_cmd
navigation command (one of MAV_CMD_NAV_FENCE_*)
Definition: navigation.h:213
uint16_t num_items
total number of items stored (excluding this one)
Definition: navigation.h:195
int16_t do_jump_mission_index
index where the do jump will go to
Definition: navigation.h:167
uint16_t update_counter
This counter is increased when (some) items change (this can wrap)
Definition: navigation.h:196
int32_t current_seq
Definition: mission.h:54
__EXPORT ssize_t dm_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, size_t count)
Write to the data manager file.
Definition: dataman.cpp:1072
uint8_t frame
MAV_FRAME.
Definition: navigation.h:227
float pitch_min
minimal pitch angle for fixed wing takeoff waypoints
Definition: navigation.h:152
uint16_t loiter_exit_xtrack
exit xtrack location: 0 for center of loiter wp, 1 for exit location
Definition: navigation.h:177
bool update(void *dst)
Update the struct.
uint16_t force_heading
heading needs to be reached
Definition: navigation.h:177
float yaw
in radians NED -PI..+PI, NAN means don&#39;t change yaw
Definition: navigation.h:157
float loiter_radius
loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise
Definition: navigation.h:156
uint8_t frame
MAV_FRAME.
Definition: navigation.h:214
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint16_t altitude_is_relative
true if altitude is relative from start point
Definition: navigation.h:177
double lat
latitude in degrees
Definition: navigation.h:146