PX4 Firmware
PX4 Autopilot Software http://px4.io
mavlink_mission.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2015 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.h
36  * Implementation of the MAVLink mission protocol.
37  * Documentation:
38  * - http://qgroundcontrol.org/mavlink/mission_interface
39  * - http://qgroundcontrol.org/mavlink/waypoint_protocol
40  *
41  * @author Lorenz Meier <lorenz@px4.io>
42  * @author Julian Oes <julian@px4.io>
43  * @author Anton Babushkin <anton@px4.io>
44  */
45 
46 #pragma once
47 
48 #include <dataman/dataman.h>
49 #include <uORB/Publication.hpp>
50 #include <uORB/Subscription.hpp>
52 
53 #include "mavlink_bridge_header.h"
54 #include "mavlink_rate_limiter.h"
55 
61 };
62 
70 };
71 
72 static constexpr uint64_t MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT = 5000000; ///< Protocol action timeout in us
73 static constexpr uint64_t MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT = 250000; ///< Protocol retry timeout in us
74 
75 class Mavlink;
76 
78 {
79 public:
80  explicit MavlinkMissionManager(Mavlink *mavlink);
81 
82  ~MavlinkMissionManager() = default;
83 
84  /**
85  * Handle sending of messages. Call this regularly at a fixed frequency.
86  * @param t current time
87  */
88  void send(const hrt_abstime t);
89 
90  void handle_message(const mavlink_message_t *msg);
91 
92  void check_active_mission(void);
93 
94 private:
95  enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE}; ///< Current state
96  enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible)
97 
98  uint64_t _time_last_recv{0};
99  uint64_t _time_last_sent{0};
100 
101  uint8_t _reached_sent_count{0}; ///< last time when the vehicle reached a waypoint
102 
103  bool _int_mode{false}; ///< Use accurate int32 instead of float
104 
105  unsigned _filesystem_errcount{0}; ///< File system error count
106 
107  static dm_item_t _dataman_id; ///< Global Dataman storage ID for active mission
108  dm_item_t _my_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_0}; ///< class Dataman storage ID
109 
110  static bool _dataman_init; ///< Dataman initialized
111 
112  static uint16_t _count[3]; ///< Count of items in (active) mission for each MAV_MISSION_TYPE
113  static int32_t _current_seq; ///< Current item sequence in active mission
114 
115  int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached)
116 
117  dm_item_t _transfer_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1}; ///< Dataman storage ID for current transmission
118 
119  uint16_t _transfer_count{0}; ///< Items count in current transmission
120  uint16_t _transfer_seq{0}; ///< Item sequence in current transmission
121 
122  int32_t _transfer_current_seq{-1}; ///< Current item ID for current transmission (-1 means not initialized)
123 
124  uint8_t _transfer_partner_sysid{0}; ///< Partner system ID for current transmission
125  uint8_t _transfer_partner_compid{0}; ///< Partner component ID for current transmission
126 
127  static bool _transfer_in_progress; ///< Global variable checking for current transmission
128 
130 
132 
133  static uint16_t _geofence_update_counter;
134  static uint16_t _safepoint_update_counter;
135  bool _geofence_locked{false}; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress)
136 
137  MavlinkRateLimiter _slow_rate_limiter{100 * 1000}; ///< Rate limit sending of the current WP sequence to 10 Hz
138 
140 
141  static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT =
142  2; ///< Error count limit before stopping to report FS errors
143  static constexpr uint16_t MAX_COUNT[] = {
147  }; /**< Maximum number of mission items for each type
148  (fence & safe points use the first item for the stats) */
149 
150  /** get the maximum number of item count for the current _mission_type */
151  uint16_t current_max_item_count();
152 
153  /** get the number of item count for the current _mission_type */
154  uint16_t current_item_count();
155 
156  /* do not allow top copying this class */
159 
160  void init_offboard_mission();
161 
162  int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq);
163 
164  /** store the geofence count to dataman */
165  int update_geofence_count(unsigned count);
166 
167  /** store the safepoint count to dataman */
168  int update_safepoint_count(unsigned count);
169 
170  /** load geofence stats from dataman */
171  int load_geofence_stats();
172 
173  /** load safe point stats from dataman */
174  int load_safepoint_stats();
175 
176  /**
177  * @brief Sends an waypoint ack message
178  */
179  void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type);
180 
181  /**
182  * @brief Broadcasts the new target waypoint and directs the MAV to fly there
183  *
184  * This function broadcasts its new active waypoint sequence number and
185  * sends a message to the controller, advising it to fly to the coordinates
186  * of the waypoint with a given orientation
187  *
188  * @param seq The waypoint sequence number the MAV should fly to.
189  */
190  void send_mission_current(uint16_t seq);
191 
192  void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type);
193 
194  void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq);
195 
196  void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq);
197 
198  /**
199  * @brief emits a message that a waypoint reached
200  *
201  * This function broadcasts a message that a waypoint is reached.
202  *
203  * @param seq The waypoint sequence number the MAV has reached.
204  */
205  void send_mission_item_reached(uint16_t seq);
206 
207  void handle_mission_ack(const mavlink_message_t *msg);
208 
209  void handle_mission_set_current(const mavlink_message_t *msg);
210 
211  void handle_mission_request_list(const mavlink_message_t *msg);
212 
213  void handle_mission_request(const mavlink_message_t *msg);
214  void handle_mission_request_int(const mavlink_message_t *msg);
215  void handle_mission_request_both(const mavlink_message_t *msg);
216 
217  void handle_mission_count(const mavlink_message_t *msg);
218 
219  void handle_mission_item(const mavlink_message_t *msg);
220  void handle_mission_item_int(const mavlink_message_t *msg);
221  void handle_mission_item_both(const mavlink_message_t *msg);
222 
223  void handle_mission_clear_all(const mavlink_message_t *msg);
224 
225  /**
226  * Parse mavlink MISSION_ITEM message to get mission_item_s.
227  *
228  * @param mavlink_mission_item pointer to mavlink_mission_item_t or mavlink_mission_item_int_t
229  * depending on _int_mode
230  * @param mission_item pointer to mission_item to construct
231  */
232  int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
233 
234  /**
235  * Format mission_item_s as mavlink MISSION_ITEM(_INT) message.
236  *
237  * @param mission_item: pointer to the existing mission item
238  * @param mavlink_mission_item: pointer to mavlink_mission_item_t or mavlink_mission_item_int_t
239  * depending on _int_mode.
240  */
241  int format_mavlink_mission_item(const struct mission_item_s *mission_item,
242  mavlink_mission_item_t *mavlink_mission_item);
243 
244  /**
245  * set _state to idle (and do necessary cleanup)
246  */
247  void switch_to_idle_state();
248 };
Global position setpoint in WGS84 coordinates.
Definition: navigation.h:145
dm_item_t
Types of items that the data manager can store.
Definition: dataman.h:50
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
DATAMANAGER driver.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58