Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
waypoints.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Lodewijk Sikkel <l.n.c.sikkel@tudelft.nl>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
29 // Include own header
33 
34 // include mavlink headers, but ignore some warnings
35 #pragma GCC diagnostic push
36 #pragma GCC diagnostic ignored "-Wswitch-default"
37 #include "mavlink/paparazzi/mavlink.h"
38 #pragma GCC diagnostic pop
39 
40 #include "generated/flight_plan.h"
41 
42 
44 //#include "subsystems/navigation/common_nav.h" // for fixed-wing aircraft
45 
47 
48 static void mavlink_send_wp(uint8_t sysid, uint8_t compid, uint16_t seq)
49 {
50  if (seq < NB_WAYPOINT) { // Due to indexing
51 #ifdef AP
52  /* for fixedwing firmware send as LOCAL_ENU for now */
53  mavlink_msg_mission_item_send(MAVLINK_COMM_0,
54  sysid,
55  compid,
56  seq,
57  MAV_FRAME_LOCAL_ENU,
58  MAV_CMD_NAV_WAYPOINT,
59  0, // current
60  0, // autocontinue
61  0, 0, 0, 0, // params
62  WaypointX(seq),
63  WaypointY(seq),
64  WaypointAlt(seq));
65 #else
66  /* for rotorcraft firmware use waypoint API and send as lat/lon */
67  /* sending lat/lon as float is actually a bad idea,
68  * but it seems that most GCSs don't understand the MISSION_ITEM_INT
69  */
70  struct LlaCoor_i *lla = waypoint_get_lla(seq);
71  mavlink_msg_mission_item_send(MAVLINK_COMM_0,
72  sysid,
73  compid,
74  seq,
75  MAV_FRAME_GLOBAL,
76  MAV_CMD_NAV_WAYPOINT,
77  0, // current
78  0, // autocontinue
79  0, 0, 0, 0, // params
80  (float)lla->lat / 1e7,
81  (float)lla->lon / 1e7,
82  (float)lla->alt / 1e3);
83 #endif
85  MAVLINK_DEBUG("Sent MISSION_ITEM message with seq %i\n", seq);
86  } else {
87  MAVLINK_DEBUG("ERROR: Wp index %i out of bounds\n", seq);
88  }
89 }
90 
91 void mavlink_wp_message_handler(const mavlink_message_t *msg)
92 {
93  switch (msg->msgid) {
94 
95  /* request for mission list, answer with number of waypoints */
96  case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
97  MAVLINK_DEBUG("Received MISSION_REQUEST_LIST message\n");
98  mavlink_mission_request_list_t mission_request_list_msg;
99  mavlink_msg_mission_request_list_decode(msg, &mission_request_list_msg);
100  if (mission_request_list_msg.target_system == mavlink_system.sysid) {
101  if (mission_mgr.state == STATE_IDLE) {
102  if (NB_WAYPOINT > 0) {
104  MAVLINK_DEBUG("State: %d\n", mission_mgr.state);
105  mission_mgr.seq = 0;
106  mission_mgr.rem_sysid = msg->sysid;
107  mission_mgr.rem_compid = msg->compid;
108  }
109  mavlink_msg_mission_count_send(MAVLINK_COMM_0, msg->sysid, msg->compid, NB_WAYPOINT);
111 
112  // Register the timeout timer (it is continuous so it needs to be cancelled after triggering)
114  } else {
115  // TODO: Handle case when the state is not IDLE
116  }
117  }
118  break;
119  }
120 
121  /* request for mission item, answer with waypoint */
122  case MAVLINK_MSG_ID_MISSION_REQUEST: {
123  mavlink_mission_request_t req;
124  mavlink_msg_mission_request_decode(msg, &req);
125  MAVLINK_DEBUG("Received MISSION_REQUEST message with seq %i\n", req.seq);
126 
127  if (req.target_system != mavlink_system.sysid || req.seq >= NB_WAYPOINT) {
128  return;
129  }
130  /* Send if:
131  * - the first waypoint
132  * - current waypoint requested again
133  * - or next waypoint requested
134  */
135  if ((mission_mgr.state == STATE_SEND_LIST && req.seq == 0) ||
136  (mission_mgr.state == STATE_SEND_ITEM && (req.seq == mission_mgr.seq ||
137  req.seq == mission_mgr.seq + 1))) {
138  // Cancel the timeout timer
140 
142  MAVLINK_DEBUG("State: %d\n", mission_mgr.state);
143  mission_mgr.seq = req.seq;
144 
145  mavlink_send_wp(msg->sysid, msg->compid, mission_mgr.seq);
146 
147  // Register the timeout timer
149  } else {
150  // TODO: Handle cases for which the above condition does not hold
151  }
152  break;
153  }
154 
155 #ifndef AP
156  /* change waypoints: only available when using waypoint API (rotorcraft firmware)
157  * This uses the waypoint_set_x functions (opposed to waypoint_move_x),
158  * meaning it doesn't send WP_MOVED Paparazzi messages.
159  */
160 
161  /* initiate mission/waypoint write transaction */
162  case MAVLINK_MSG_ID_MISSION_COUNT: {
163  mavlink_mission_count_t mission_count;
164  mavlink_msg_mission_count_decode(msg, &mission_count);
165  if (mission_count.target_system != mavlink_system.sysid) {
166  return;
167  }
168  MAVLINK_DEBUG("Received MISSION_COUNT message with count %i\n", mission_count.count);
169  /* only allow new waypoint update transaction if currently idle */
170  if (mission_mgr.state != STATE_IDLE) {
171  MAVLINK_DEBUG("MISSION_COUNT error: mission manager not idle.\n");
172  return;
173  }
174  if (mission_count.count != NB_WAYPOINT) {
175  MAVLINK_DEBUG("MISSION_COUNT error: request writing %i instead of %i waypoints\n",
176  mission_count.count, NB_WAYPOINT);
177  return;
178  }
179  /* valid initiation of waypoint write transaction, ask for first waypoint */
180  MAVLINK_DEBUG("MISSION_COUNT: Requesting first waypoint\n");
181  mavlink_msg_mission_request_send(MAVLINK_COMM_0, msg->sysid, msg->compid, 0);
183 
184  mission_mgr.seq = 0;
186 
187  // Register the timeout timer
189  }
190  break;
191 
192  /* got MISSION_ITEM, update one waypoint if in valid transaction */
193  case MAVLINK_MSG_ID_MISSION_ITEM: {
194  mavlink_mission_item_t mission_item;
195  mavlink_msg_mission_item_decode(msg, &mission_item);
196 
197  if (mission_item.target_system != mavlink_system.sysid) {
198  return;
199  }
200 
201  MAVLINK_DEBUG("Received MISSION_ITEM message with seq %i and frame %i\n",
202  mission_item.seq, mission_item.frame);
203 
204  /* reject non waypoint updates */
205  if (mission_item.command != MAV_CMD_NAV_WAYPOINT ||
206  mission_item.seq >= NB_WAYPOINT) {
207  MAVLINK_DEBUG("rejected MISSION_ITEM command %i, seq %i\n",
208  mission_item.command, mission_item.seq);
209  return;
210  }
211 
212  /* Only accept mission item in IDLE (update single waypoint) or in write transaction */
214  MAVLINK_DEBUG("got MISSION_ITEM while not in waypoint write transaction or idle\n");
215  return;
216  }
217  /* If in write transaction, only handle mission item if correct sequence */
218  if (mission_mgr.state == STATE_WAYPOINT_WRITE_TRANSACTION && mission_item.seq != mission_mgr.seq) {
219  MAVLINK_DEBUG("MISSION_ITEM, got waypoint seq %i, but requested %i\n",
220  mission_item.seq, mission_mgr.seq);
221  return;
222  }
223 
224  if (mission_item.frame == MAV_FRAME_GLOBAL) {
225  MAVLINK_DEBUG("MISSION_ITEM, global wp: lat=%f, lon=%f, alt=%f\n",
226  mission_item.x, mission_item.y, mission_item.z);
227  struct LlaCoor_i lla;
228  lla.lat = mission_item.x * 1e7; // lattitude in degrees*1e7
229  lla.lon = mission_item.y * 1e7; // longitude in degrees*1e7
230  lla.alt = mission_item.z * 1e3; // altitude in millimeters
231  waypoint_set_lla(mission_item.seq, &lla);
232  }
233  else if (mission_item.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
234  MAVLINK_DEBUG("MISSION_ITEM, global_rel_alt wp: lat=%f, lon=%f, relative alt=%f\n",
235  mission_item.x, mission_item.y, mission_item.z);
236  struct LlaCoor_i lla;
237  lla.lat = mission_item.x * 1e7; // lattitude in degrees*1e7
238  lla.lon = mission_item.y * 1e7; // longitude in degrees*1e7
239  lla.alt = state.ned_origin_i.hmsl + mission_item.z * 1e3; // altitude in millimeters
240  waypoint_set_lla(mission_item.seq, &lla);
241  }
242  else if (mission_item.frame == MAV_FRAME_LOCAL_ENU) {
243  MAVLINK_DEBUG("MISSION_ITEM, local_enu wp: x=%f, y=%f, z=%f\n",
244  mission_item.x, mission_item.y, mission_item.z);
245  struct EnuCoor_f enu;
246  enu.x = mission_item.x;
247  enu.y = mission_item.y;
248  enu.z = mission_item.z;
249  waypoint_set_enu(mission_item.seq, &enu);
250  }
251  else {
252  MAVLINK_DEBUG("No handler for MISSION_ITEM with frame %i\n", mission_item.frame);
253  return;
254  }
255  // acknowledge transmission single waypoint update
256  if (mission_mgr.state == STATE_IDLE) {
257  MAVLINK_DEBUG("Acknowledge single waypoint update\n");
258  mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid,
259  MAV_MISSION_ACCEPTED);
262  }
263  // or end of transaction
264  else if (mission_item.seq == NB_WAYPOINT -1) {
265  MAVLINK_DEBUG("Acknowledging end of waypoint write transaction\n");
266  mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid,
267  MAV_MISSION_ACCEPTED);
271  }
272  // or request next waypoint if still in middle of transaction
273  else {
274  MAVLINK_DEBUG("Requesting waypoint %i\n", mission_item.seq + 1);
275  mavlink_msg_mission_request_send(MAVLINK_COMM_0, msg->sysid, msg->compid,
276  mission_item.seq + 1);
278  mission_mgr.seq = mission_item.seq + 1;
280  }
281  break;
282  }
283 
284  case MAVLINK_MSG_ID_MISSION_ITEM_INT: {
285  mavlink_mission_item_int_t mission_item;
286  mavlink_msg_mission_item_int_decode(msg, &mission_item);
287 
288  if (mission_item.target_system == mavlink_system.sysid) {
289  MAVLINK_DEBUG("Received MISSION_ITEM_INT message with seq %i and frame %i\n",
290  mission_item.seq, mission_item.frame);
291  if (mission_item.seq >= NB_WAYPOINT) {
292  return;
293  }
294  if (mission_item.frame == MAV_FRAME_GLOBAL_INT) {
295  MAVLINK_DEBUG("MISSION_ITEM_INT, global_int wp: lat=%i, lon=%i, alt=%f\n",
296  mission_item.x, mission_item.y, mission_item.z);
297  struct LlaCoor_i lla;
298  lla.lat = mission_item.x; // lattitude in degrees*1e7
299  lla.lon = mission_item.y; // longitude in degrees*1e7
300  lla.alt = mission_item.z * 1e3; // altitude in millimeters
301  waypoint_set_lla(mission_item.seq, &lla);
302  mavlink_msg_mission_ack_send(MAVLINK_COMM_0, msg->sysid, msg->compid,
303  MAV_MISSION_ACCEPTED);
305  }
306  else {
307  MAVLINK_DEBUG("No handler for MISSION_ITEM_INT with frame %i\n", mission_item.frame);
308  return;
309  }
310  }
311  }
312  break;
313 #endif // AP
314 
315  default:
316  break;
317  }
318 }
unsigned short uint16_t
Definition: types.h:16
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
vector in East North Up coordinates Units: meters
static int seq
Definition: nps_ivy.c:33
vector in Latitude, Longitude and Altitude
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
Common functions used within the mission library, blocks and waypoints cannot be send simultaneously ...
#define WaypointX(_wp)
Definition: common_nav.h:45
float x
in meters
int32_t lon
in degrees*1e7
#define WaypointY(_wp)
Definition: common_nav.h:46
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
void mavlink_mission_cancel_timer(void)
void mavlink_mission_set_timer(void)
unsigned char uint8_t
Definition: types.h:14
float z
in meters
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition: waypoints.c:113
struct LlaCoor_i * waypoint_get_lla(uint8_t wp_id)
Get LLA coordinates of waypoint.
Definition: waypoints.c:310
enum MAVLINK_MISSION_MGR_STATES state
int32_t lat
in degrees*1e7
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
float y
in meters
struct State state
Definition: state.c:36
void waypoint_set_lla(uint8_t wp_id, struct LlaCoor_i *lla)
Definition: waypoints.c:179