Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
mission_common.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Paparazzi Team
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 
28 
29 #include <string.h>
31 #include "generated/flight_plan.h"
32 #include "generated/airframe.h"
35 
37 
38 void mission_init(void)
39 {
40  mission.insert_idx = 0;
41  mission.current_idx = 0;
42  mission.element_time = 0.;
43 }
44 
45 
46 // Insert element
47 bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element *element)
48 {
49  uint8_t tmp;
50  // convert element if needed, return FALSE if failed
51  if (!mission_element_convert(element)) { return FALSE; }
52 
53  switch (insert) {
54  case Append:
56  if (tmp == mission.current_idx) { return FALSE; } // no room to insert element
57  mission.elements[mission.insert_idx] = *element; // add element
58  mission.insert_idx = tmp; // move insert index
59  break;
60  case Prepend:
61  if (mission.current_idx == 0) { tmp = MISSION_ELEMENT_NB - 1; }
62  else { tmp = mission.current_idx - 1; }
63  if (tmp == mission.insert_idx) { return FALSE; } // no room to inser element
64  mission.elements[tmp] = *element; // add element
65  mission.current_idx = tmp; // move current index
66  break;
67  case ReplaceCurrent:
68  // current element can always be modified, index are not changed
69  mission.elements[mission.current_idx] = *element;
70  break;
71  case ReplaceAll:
72  // reset queue and index
73  mission.elements[0] = *element;
74  mission.current_idx = 0;
75  mission.insert_idx = 1;
76  break;
77  default:
78  // unknown insertion mode
79  return FALSE;
80  }
81  return TRUE;
82 
83 }
84 
85 
86 // Weak implementation of mission_element_convert (leave element unchanged)
87 bool_t __attribute__((weak)) mission_element_convert(struct _mission_element *el __attribute__((unused))) { return TRUE; }
88 
89 
90 // Get element
92 {
94  return NULL;
95  }
97 }
98 
99 
100 // Report function
102 {
103  // build task list
104  uint8_t task_list[MISSION_ELEMENT_NB];
105  uint8_t i = mission.current_idx, j = 0;
106  while (i != mission.insert_idx) {
107  task_list[j++] = (uint8_t)mission.elements[i].type;
108  i = (i + 1) % MISSION_ELEMENT_NB;
109  }
110  if (j == 0) { task_list[j++] = 0; } // Dummy value if task list is empty
111  //compute remaining time (or -1. if no time limit)
112  float remaining_time = -1.;
115  }
116 
117  // send status
118  DOWNLINK_SEND_MISSION_STATUS(DefaultChannel, DefaultDevice, &remaining_time, j, task_list);
119 }
120 
121 
123 // Parsing functions //
125 
127 {
128  if (DL_MISSION_GOTO_WP_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft
129 
130  struct _mission_element me;
131  me.type = MissionWP;
132  me.element.mission_wp.wp.wp_f.x = DL_MISSION_GOTO_WP_wp_east(dl_buffer);
133  me.element.mission_wp.wp.wp_f.y = DL_MISSION_GOTO_WP_wp_north(dl_buffer);
134  me.element.mission_wp.wp.wp_f.z = DL_MISSION_GOTO_WP_wp_alt(dl_buffer);
135  me.duration = DL_MISSION_GOTO_WP_duration(dl_buffer);
136 
137  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_insert(dl_buffer));
138 
139  return mission_insert(insert, &me);
140 }
141 
143 {
144  if (DL_MISSION_GOTO_WP_LLA_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft
145 
146  struct LlaCoor_i lla;
147  lla.lat = DL_MISSION_GOTO_WP_LLA_wp_lat(dl_buffer);
148  lla.lon = DL_MISSION_GOTO_WP_LLA_wp_lon(dl_buffer);
149  lla.alt = DL_MISSION_GOTO_WP_LLA_wp_alt(dl_buffer);
150 
151  struct _mission_element me;
152  me.type = MissionWP;
153  // if there is no valid local coordinate, do not insert mission element
154  if (!mission_point_of_lla(&me.element.mission_wp.wp.wp_f, &lla)) { return FALSE; }
155  me.duration = DL_MISSION_GOTO_WP_LLA_duration(dl_buffer);
156 
157  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_LLA_insert(dl_buffer));
158 
159  return mission_insert(insert, &me);
160 }
161 
163 {
164  if (DL_MISSION_CIRCLE_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft
165 
166  struct _mission_element me;
167  me.type = MissionCircle;
168  me.element.mission_circle.center.center_f.x = DL_MISSION_CIRCLE_center_east(dl_buffer);
169  me.element.mission_circle.center.center_f.y = DL_MISSION_CIRCLE_center_north(dl_buffer);
170  me.element.mission_circle.center.center_f.z = DL_MISSION_CIRCLE_center_alt(dl_buffer);
171  me.element.mission_circle.radius = DL_MISSION_CIRCLE_radius(dl_buffer);
172  me.duration = DL_MISSION_CIRCLE_duration(dl_buffer);
173 
174  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_insert(dl_buffer));
175 
176  return mission_insert(insert, &me);
177 }
178 
180 {
181  if (DL_MISSION_CIRCLE_LLA_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft
182 
183  struct LlaCoor_i lla;
184  lla.lat = DL_MISSION_CIRCLE_LLA_center_lat(dl_buffer);
185  lla.lon = DL_MISSION_CIRCLE_LLA_center_lon(dl_buffer);
186  lla.alt = DL_MISSION_CIRCLE_LLA_center_alt(dl_buffer);
187 
188  struct _mission_element me;
189  me.type = MissionCircle;
190  // if there is no valid local coordinate, do not insert mission element
191  if (!mission_point_of_lla(&me.element.mission_circle.center.center_f, &lla)) { return FALSE; }
192  me.element.mission_circle.radius = DL_MISSION_CIRCLE_LLA_radius(dl_buffer);
193  me.duration = DL_MISSION_CIRCLE_LLA_duration(dl_buffer);
194 
195  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_LLA_insert(dl_buffer));
196 
197  return mission_insert(insert, &me);
198 }
199 
201 {
202  if (DL_MISSION_SEGMENT_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft
203 
204  struct _mission_element me;
205  me.type = MissionSegment;
206  me.element.mission_segment.from.from_f.x = DL_MISSION_SEGMENT_segment_east_1(dl_buffer);
207  me.element.mission_segment.from.from_f.y = DL_MISSION_SEGMENT_segment_north_1(dl_buffer);
208  me.element.mission_segment.from.from_f.z = DL_MISSION_SEGMENT_segment_alt(dl_buffer);
209  me.element.mission_segment.to.to_f.x = DL_MISSION_SEGMENT_segment_east_2(dl_buffer);
210  me.element.mission_segment.to.to_f.y = DL_MISSION_SEGMENT_segment_north_2(dl_buffer);
211  me.element.mission_segment.to.to_f.z = DL_MISSION_SEGMENT_segment_alt(dl_buffer);
212  me.duration = DL_MISSION_SEGMENT_duration(dl_buffer);
213 
214  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_insert(dl_buffer));
215 
216  return mission_insert(insert, &me);
217 }
218 
220 {
221  if (DL_MISSION_SEGMENT_LLA_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft
222 
223  struct LlaCoor_i from_lla, to_lla;
224  from_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_1(dl_buffer);
225  from_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_1(dl_buffer);
226  from_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(dl_buffer);
227  to_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_2(dl_buffer);
228  to_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_2(dl_buffer);
229  to_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(dl_buffer);
230 
231  struct _mission_element me;
232  me.type = MissionSegment;
233  // if there is no valid local coordinate, do not insert mission element
234  if (!mission_point_of_lla(&me.element.mission_segment.from.from_f, &from_lla)) { return FALSE; }
235  if (!mission_point_of_lla(&me.element.mission_segment.to.to_f, &to_lla)) { return FALSE; }
236  me.duration = DL_MISSION_SEGMENT_LLA_duration(dl_buffer);
237 
238  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_LLA_insert(dl_buffer));
239 
240  return mission_insert(insert, &me);
241 }
242 
244 {
245  if (DL_MISSION_PATH_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft
246 
247  struct _mission_element me;
248  me.type = MissionPath;
249  me.element.mission_path.path.path_f[0].x = DL_MISSION_PATH_point_east_1(dl_buffer);
250  me.element.mission_path.path.path_f[0].y = DL_MISSION_PATH_point_north_1(dl_buffer);
251  me.element.mission_path.path.path_f[0].z = DL_MISSION_PATH_path_alt(dl_buffer);
252  me.element.mission_path.path.path_f[1].x = DL_MISSION_PATH_point_east_2(dl_buffer);
253  me.element.mission_path.path.path_f[1].y = DL_MISSION_PATH_point_north_2(dl_buffer);
254  me.element.mission_path.path.path_f[1].z = DL_MISSION_PATH_path_alt(dl_buffer);
255  me.element.mission_path.path.path_f[2].x = DL_MISSION_PATH_point_east_3(dl_buffer);
256  me.element.mission_path.path.path_f[2].y = DL_MISSION_PATH_point_north_3(dl_buffer);
257  me.element.mission_path.path.path_f[2].z = DL_MISSION_PATH_path_alt(dl_buffer);
258  me.element.mission_path.path.path_f[3].x = DL_MISSION_PATH_point_east_4(dl_buffer);
259  me.element.mission_path.path.path_f[3].y = DL_MISSION_PATH_point_north_4(dl_buffer);
260  me.element.mission_path.path.path_f[3].z = DL_MISSION_PATH_path_alt(dl_buffer);
261  me.element.mission_path.path.path_f[4].x = DL_MISSION_PATH_point_east_5(dl_buffer);
262  me.element.mission_path.path.path_f[4].y = DL_MISSION_PATH_point_north_5(dl_buffer);
263  me.element.mission_path.path.path_f[4].z = DL_MISSION_PATH_path_alt(dl_buffer);
264  me.element.mission_path.nb = DL_MISSION_PATH_nb(dl_buffer);
265  if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; }
266  me.element.mission_path.path_idx = 0;
267  me.duration = DL_MISSION_PATH_duration(dl_buffer);
268 
269  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_insert(dl_buffer));
270 
271  return mission_insert(insert, &me);
272 }
273 
275 {
276  if (DL_MISSION_PATH_LLA_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft
277 
278  struct LlaCoor_i lla[MISSION_PATH_NB];
279  lla[0].lat = DL_MISSION_PATH_LLA_point_lat_1(dl_buffer);
280  lla[0].lon = DL_MISSION_PATH_LLA_point_lon_1(dl_buffer);
281  lla[0].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
282  lla[1].lat = DL_MISSION_PATH_LLA_point_lat_2(dl_buffer);
283  lla[1].lon = DL_MISSION_PATH_LLA_point_lon_2(dl_buffer);
284  lla[1].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
285  lla[2].lat = DL_MISSION_PATH_LLA_point_lat_3(dl_buffer);
286  lla[2].lon = DL_MISSION_PATH_LLA_point_lon_3(dl_buffer);
287  lla[2].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
288  lla[3].lat = DL_MISSION_PATH_LLA_point_lat_4(dl_buffer);
289  lla[3].lon = DL_MISSION_PATH_LLA_point_lon_4(dl_buffer);
290  lla[3].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
291  lla[4].lat = DL_MISSION_PATH_LLA_point_lat_5(dl_buffer);
292  lla[4].lon = DL_MISSION_PATH_LLA_point_lon_5(dl_buffer);
293  lla[4].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
294 
295  struct _mission_element me;
296  me.type = MissionPath;
297  uint8_t i;
298  me.element.mission_path.nb = DL_MISSION_PATH_LLA_nb(dl_buffer);
299  if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; }
300  for (i = 0; i < me.element.mission_path.nb; i++) {
301  // if there is no valid local coordinate, do not insert mission element
302  if (!mission_point_of_lla(&me.element.mission_path.path.path_f[i], &lla[i])) { return FALSE; }
303  }
304  me.element.mission_path.path_idx = 0;
305  me.duration = DL_MISSION_PATH_LLA_duration(dl_buffer);
306 
307  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_LLA_insert(dl_buffer));
308 
309  return mission_insert(insert, &me);
310 }
311 
313 {
314  if (DL_GOTO_MISSION_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft
315 
316  uint8_t mission_id = DL_GOTO_MISSION_mission_id(dl_buffer);
317  if (mission_id < MISSION_ELEMENT_NB) {
318  mission.current_idx = mission_id;
319  } else { return FALSE; }
320 
321  return TRUE;
322 }
323 
325 {
326  if (DL_NEXT_MISSION_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft
327 
328  if (mission.current_idx == mission.insert_idx) { return FALSE; } // already at the last position
329 
330  // increment current index
332  return TRUE;
333 }
334 
336 {
337  if (DL_END_MISSION_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft
338 
339  // set current index to insert index (last position)
341  return TRUE;
342 }
343 
int mission_parse_NEXT_MISSION(void)
int mission_parse_SEGMENT(void)
struct _mission_element * mission_get(void)
Get current mission element.
bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla)
Get the ENU component of LLA mission point This function is firmware specific.
void mission_init(void)
Init mission structure.
void mission_status_report(void)
Report mission status.
MissionInsertMode
remove all elements and add the new one
float duration
time to spend in the element (<= 0 to disable)
add before the current element
vector in Latitude, Longitude and Altitude
replace current element
int mission_parse_GOTO_WP(void)
Parsing functions called when a mission message is received.
int mission_parse_CIRCLE(void)
#define FALSE
Definition: std.h:5
int32_t alt
in millimeters above WGS84 reference ellipsoid
union _mission_element::@21 element
float element_time
time in second spend in the current element
#define TRUE
Definition: std.h:4
int mission_parse_GOTO_WP_LLA(void)
#define MISSION_ELEMENT_NB
Max number of elements in the tasks' list can be redefined.
mission planner library
add at the last position
int mission_parse_PATH(void)
int32_t lon
in degrees*1e7
uint8_t insert_idx
inserstion index
int mission_parse_SEGMENT_LLA(void)
bool_t mission_element_convert(struct _mission_element *el)
Convert mission element's points format if needed.
unsigned char uint8_t
Definition: types.h:14
bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element *element)
Insert a mission element according to the insertion mode.
int mission_parse_GOTO_MISSION(void)
#define MISSION_PATH_NB
uint8_t current_idx
current mission element index
struct _mission mission
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
struct _mission_element elements[MISSION_ELEMENT_NB]
int32_t lat
in degrees*1e7
int mission_parse_PATH_LLA(void)
int mission_parse_CIRCLE_LLA(void)
int mission_parse_END_MISSION(void)
enum MissionType type