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
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>
30 #include "generated/flight_plan.h"
31 #include "generated/airframe.h"
34 
35 struct _mission mission = { 0 };
36 
37 void mission_init(void)
38 {
39  mission.insert_idx = 0;
40  mission.current_idx = 0;
41  mission.element_time = 0.;
42 
43  // FIXME
44  // we have no guarantee that nav modules init are called after mission_init
45  // this would erase the already registered elements
46  // for now, rely on the static initialization
47  //for (int i = 0; i < MISSION_REGISTER_NB; i++) {
48  // mission.registered[i].cb = NULL;
49  // memset(mission.registered[i].type, '\0', MISSION_TYPE_SIZE);
50  //}
51 }
52 
53 
54 // Insert element
55 bool mission_insert(enum MissionInsertMode insert, struct _mission_element *element)
56 {
57  uint8_t tmp;
58  // convert element if needed, return FALSE if failed
59  if (!mission_element_convert(element)) { return false; }
60 
61  switch (insert) {
62  case Append:
63  tmp = (mission.insert_idx + 1) % MISSION_ELEMENT_NB;
64  if (tmp == mission.current_idx) { return false; } // no room to insert element
65  mission.elements[mission.insert_idx] = *element; // add element
66  mission.insert_idx = tmp; // move insert index
67  break;
68  case Prepend:
69  if (mission.current_idx == 0) { tmp = MISSION_ELEMENT_NB - 1; }
70  else { tmp = mission.current_idx - 1; }
71  if (tmp == mission.insert_idx) { return false; } // no room to inser element
72  mission.elements[tmp] = *element; // add element
73  mission.current_idx = tmp; // move current index
74  break;
75  case ReplaceCurrent:
76  // current element can always be modified, index are not changed
77  mission.elements[mission.current_idx] = *element;
78  break;
79  case ReplaceAll:
80  // reset queue and index
81  mission.elements[0] = *element;
82  mission.current_idx = 0;
83  mission.insert_idx = 1;
84  break;
85  case ReplaceNexts:
86  tmp = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
87  mission.elements[tmp] = *element;
88  mission.insert_idx = (mission.current_idx + 2) % MISSION_ELEMENT_NB;
89  break;
90  default:
91  // unknown insertion mode
92  return false;
93  }
94  return true;
95 
96 }
97 
98 // Register new callback
99 bool mission_register(mission_custom_cb cb, char *type)
100 {
101  for (int i = 0; i < MISSION_REGISTER_NB; i++) {
102  if (str_equal(mission.registered[i].type, type)) {
103  return false; // identifier already registered
104  }
105  if (mission.registered[i].cb == NULL) {
106  strncpy(mission.registered[i].type, type, MISSION_TYPE_SIZE-1);
107  mission.registered[i].cb = cb;
108  return true;
109  }
110  }
111  return false; // no more room to register callbacks
112 }
113 
114 // Returns a pointer to a register struct with matching types, NULL if not found
116 {
117  for (int i = 0; i < MISSION_REGISTER_NB; i++) {
118  if (str_equal(mission.registered[i].type, type)) {
119  return &(mission.registered[i]);
120  }
121  }
122  return NULL; // not found
123 }
124 
125 // Weak implementation of mission_element_convert (leave element unchanged)
126 bool __attribute__((weak)) mission_element_convert(struct _mission_element *el __attribute__((unused))) { return true; }
127 
128 
129 // Get element
131 {
132  if (mission.current_idx == mission.insert_idx) {
133  return NULL;
134  }
135  return &(mission.elements[mission.current_idx]);
136 }
137 
138 
139 // Report function
141 {
142  // build index list
143  uint8_t index_list[MISSION_ELEMENT_NB];
144  uint8_t i = mission.current_idx, j = 0;
145  while (i != mission.insert_idx) {
146  index_list[j++] = mission.elements[i].index;
147  i = (i + 1) % MISSION_ELEMENT_NB;
148  }
149  if (j == 0) { index_list[j++] = 0; } // Dummy value if index list is empty
150  //compute remaining time (or -1. if no time limit)
151  float remaining_time = -1.;
152  if (mission.elements[mission.current_idx].duration > 0.) {
153  remaining_time = mission.elements[mission.current_idx].duration - mission.element_time;
154  }
155 
156  // send status
157  DOWNLINK_SEND_MISSION_STATUS(DefaultChannel, DefaultDevice, &remaining_time, j, index_list);
158 }
159 
160 
162 // Parsing functions //
164 
166 {
167  if (DL_MISSION_GOTO_WP_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
168 
169  struct _mission_element me;
170  me.type = MissionWP;
171  me.element.mission_wp.wp.wp_f.x = DL_MISSION_GOTO_WP_wp_east(buf);
172  me.element.mission_wp.wp.wp_f.y = DL_MISSION_GOTO_WP_wp_north(buf);
173  me.element.mission_wp.wp.wp_f.z = DL_MISSION_GOTO_WP_wp_alt(buf);
174  me.duration = DL_MISSION_GOTO_WP_duration(buf);
175  me.index = DL_MISSION_GOTO_WP_index(buf);
176 
177  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_insert(buf));
178 
179  return mission_insert(insert, &me);
180 }
181 
183 {
184  if (DL_MISSION_GOTO_WP_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
185 
186  struct LlaCoor_i lla;
187  lla.lat = DL_MISSION_GOTO_WP_LLA_wp_lat(buf);
188  lla.lon = DL_MISSION_GOTO_WP_LLA_wp_lon(buf);
189  lla.alt = DL_MISSION_GOTO_WP_LLA_wp_alt(buf);
190 
191  struct _mission_element me;
192  me.type = MissionWP;
193  // if there is no valid local coordinate, do not insert mission element
194  if (!mission_point_of_lla(&me.element.mission_wp.wp.wp_f, &lla)) { return false; }
195  me.duration = DL_MISSION_GOTO_WP_LLA_duration(buf);
196  me.index = DL_MISSION_GOTO_WP_LLA_index(buf);
197 
198  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_LLA_insert(buf));
199 
200  return mission_insert(insert, &me);
201 }
202 
204 {
205  if (DL_MISSION_CIRCLE_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
206 
207  struct _mission_element me;
208  me.type = MissionCircle;
209  me.element.mission_circle.center.center_f.x = DL_MISSION_CIRCLE_center_east(buf);
210  me.element.mission_circle.center.center_f.y = DL_MISSION_CIRCLE_center_north(buf);
211  me.element.mission_circle.center.center_f.z = DL_MISSION_CIRCLE_center_alt(buf);
212  me.element.mission_circle.radius = DL_MISSION_CIRCLE_radius(buf);
213  me.duration = DL_MISSION_CIRCLE_duration(buf);
214  me.index = DL_MISSION_CIRCLE_index(buf);
215 
216  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_insert(buf));
217 
218  return mission_insert(insert, &me);
219 }
220 
222 {
223  if (DL_MISSION_CIRCLE_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
224 
225  struct LlaCoor_i lla;
226  lla.lat = DL_MISSION_CIRCLE_LLA_center_lat(buf);
227  lla.lon = DL_MISSION_CIRCLE_LLA_center_lon(buf);
228  lla.alt = DL_MISSION_CIRCLE_LLA_center_alt(buf);
229 
230  struct _mission_element me;
231  me.type = MissionCircle;
232  // if there is no valid local coordinate, do not insert mission element
233  if (!mission_point_of_lla(&me.element.mission_circle.center.center_f, &lla)) { return false; }
234  me.element.mission_circle.radius = DL_MISSION_CIRCLE_LLA_radius(buf);
235  me.duration = DL_MISSION_CIRCLE_LLA_duration(buf);
236  me.index = DL_MISSION_CIRCLE_LLA_index(buf);
237 
238  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_LLA_insert(buf));
239 
240  return mission_insert(insert, &me);
241 }
242 
244 {
245  if (DL_MISSION_SEGMENT_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
246 
247  struct _mission_element me;
248  me.type = MissionSegment;
249  me.element.mission_segment.from.from_f.x = DL_MISSION_SEGMENT_segment_east_1(buf);
250  me.element.mission_segment.from.from_f.y = DL_MISSION_SEGMENT_segment_north_1(buf);
251  me.element.mission_segment.from.from_f.z = DL_MISSION_SEGMENT_segment_alt(buf);
252  me.element.mission_segment.to.to_f.x = DL_MISSION_SEGMENT_segment_east_2(buf);
253  me.element.mission_segment.to.to_f.y = DL_MISSION_SEGMENT_segment_north_2(buf);
254  me.element.mission_segment.to.to_f.z = DL_MISSION_SEGMENT_segment_alt(buf);
255  me.duration = DL_MISSION_SEGMENT_duration(buf);
256  me.index = DL_MISSION_SEGMENT_index(buf);
257 
258  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_insert(buf));
259 
260  return mission_insert(insert, &me);
261 }
262 
264 {
265  if (DL_MISSION_SEGMENT_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
266 
267  struct LlaCoor_i from_lla, to_lla;
268  from_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_1(buf);
269  from_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_1(buf);
270  from_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(buf);
271  to_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_2(buf);
272  to_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_2(buf);
273  to_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(buf);
274 
275  struct _mission_element me;
276  me.type = MissionSegment;
277  // if there is no valid local coordinate, do not insert mission element
278  if (!mission_point_of_lla(&me.element.mission_segment.from.from_f, &from_lla)) { return false; }
279  if (!mission_point_of_lla(&me.element.mission_segment.to.to_f, &to_lla)) { return false; }
280  me.duration = DL_MISSION_SEGMENT_LLA_duration(buf);
281  me.index = DL_MISSION_SEGMENT_LLA_index(buf);
282 
283  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_LLA_insert(buf));
284 
285  return mission_insert(insert, &me);
286 }
287 
289 {
290  if (DL_MISSION_PATH_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
291 
292  struct _mission_element me;
293  me.type = MissionPath;
294  me.element.mission_path.path.path_f[0].x = DL_MISSION_PATH_point_east_1(buf);
295  me.element.mission_path.path.path_f[0].y = DL_MISSION_PATH_point_north_1(buf);
296  me.element.mission_path.path.path_f[0].z = DL_MISSION_PATH_path_alt(buf);
297  me.element.mission_path.path.path_f[1].x = DL_MISSION_PATH_point_east_2(buf);
298  me.element.mission_path.path.path_f[1].y = DL_MISSION_PATH_point_north_2(buf);
299  me.element.mission_path.path.path_f[1].z = DL_MISSION_PATH_path_alt(buf);
300  me.element.mission_path.path.path_f[2].x = DL_MISSION_PATH_point_east_3(buf);
301  me.element.mission_path.path.path_f[2].y = DL_MISSION_PATH_point_north_3(buf);
302  me.element.mission_path.path.path_f[2].z = DL_MISSION_PATH_path_alt(buf);
303  me.element.mission_path.path.path_f[3].x = DL_MISSION_PATH_point_east_4(buf);
304  me.element.mission_path.path.path_f[3].y = DL_MISSION_PATH_point_north_4(buf);
305  me.element.mission_path.path.path_f[3].z = DL_MISSION_PATH_path_alt(buf);
306  me.element.mission_path.path.path_f[4].x = DL_MISSION_PATH_point_east_5(buf);
307  me.element.mission_path.path.path_f[4].y = DL_MISSION_PATH_point_north_5(buf);
308  me.element.mission_path.path.path_f[4].z = DL_MISSION_PATH_path_alt(buf);
309  me.element.mission_path.nb = DL_MISSION_PATH_nb(buf);
310  if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; }
311  me.element.mission_path.path_idx = 0;
312  me.duration = DL_MISSION_PATH_duration(buf);
313  me.index = DL_MISSION_PATH_index(buf);
314 
315  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_insert(buf));
316 
317  return mission_insert(insert, &me);
318 }
319 
321 {
322  if (DL_MISSION_PATH_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
323 
324  struct LlaCoor_i lla[MISSION_PATH_NB];
325  lla[0].lat = DL_MISSION_PATH_LLA_point_lat_1(buf);
326  lla[0].lon = DL_MISSION_PATH_LLA_point_lon_1(buf);
327  lla[0].alt = DL_MISSION_PATH_LLA_path_alt(buf);
328  lla[1].lat = DL_MISSION_PATH_LLA_point_lat_2(buf);
329  lla[1].lon = DL_MISSION_PATH_LLA_point_lon_2(buf);
330  lla[1].alt = DL_MISSION_PATH_LLA_path_alt(buf);
331  lla[2].lat = DL_MISSION_PATH_LLA_point_lat_3(buf);
332  lla[2].lon = DL_MISSION_PATH_LLA_point_lon_3(buf);
333  lla[2].alt = DL_MISSION_PATH_LLA_path_alt(buf);
334  lla[3].lat = DL_MISSION_PATH_LLA_point_lat_4(buf);
335  lla[3].lon = DL_MISSION_PATH_LLA_point_lon_4(buf);
336  lla[3].alt = DL_MISSION_PATH_LLA_path_alt(buf);
337  lla[4].lat = DL_MISSION_PATH_LLA_point_lat_5(buf);
338  lla[4].lon = DL_MISSION_PATH_LLA_point_lon_5(buf);
339  lla[4].alt = DL_MISSION_PATH_LLA_path_alt(buf);
340 
341  struct _mission_element me;
342  me.type = MissionPath;
343  uint8_t i;
344  me.element.mission_path.nb = DL_MISSION_PATH_LLA_nb(buf);
345  if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; }
346  for (i = 0; i < me.element.mission_path.nb; i++) {
347  // if there is no valid local coordinate, do not insert mission element
348  if (!mission_point_of_lla(&me.element.mission_path.path.path_f[i], &lla[i])) { return false; }
349  }
350  me.element.mission_path.path_idx = 0;
351  me.duration = DL_MISSION_PATH_LLA_duration(buf);
352  me.index = DL_MISSION_PATH_LLA_index(buf);
353 
354  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_LLA_insert(buf));
355 
356  return mission_insert(insert, &me);
357 }
358 
360 {
361  if (DL_MISSION_CUSTOM_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
362 
363  struct _mission_element me;
364  me.type = MissionCustom;
365  me.element.mission_custom.reg = mission_get_registered(DL_MISSION_CUSTOM_type(buf));
366  if (me.element.mission_custom.reg == NULL) {
367  return false; // unknown type
368  }
369  me.element.mission_custom.nb = DL_MISSION_CUSTOM_params_length(buf);
370  for (int i = 0; i < me.element.mission_custom.nb; i++) {
371  me.element.mission_custom.params[i] = DL_MISSION_CUSTOM_params(buf)[i];
372  }
373  me.duration = DL_MISSION_CUSTOM_duration(buf);
374  me.index = DL_MISSION_CUSTOM_index(buf);
375 
376  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CUSTOM_insert(buf));
377 
378  return mission_insert(insert, &me);
379 }
380 
382 {
383  if (DL_GOTO_MISSION_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
384 
385  uint8_t mission_id = DL_GOTO_MISSION_mission_id(buf);
386  if (mission_id < MISSION_ELEMENT_NB) {
387  mission.current_idx = mission_id;
388  } else { return false; }
389 
390  return true;
391 }
392 
394 {
395  if (DL_NEXT_MISSION_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
396 
397  if (mission.current_idx == mission.insert_idx) { return false; } // already at the last position
398 
399  // increment current index
400  mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
401  return true;
402 }
403 
405 {
406  if (DL_END_MISSION_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
407 
408  // set current index to insert index (last position)
409  mission.current_idx = mission.insert_idx;
410  return true;
411 }
replace the next element and remove all the others
char type[MISSION_TYPE_SIZE]
mission element identifier (5 char max + 1 \0)
int mission_parse_SEGMENT_LLA(uint8_t *buf)
bool(* mission_custom_cb)(uint8_t nb, float *params, bool init)
custom mission element callback
struct _mission_element * mission_get(void)
Get current mission element.
struct _mission_registered registered[MISSION_REGISTER_NB]
int mission_parse_CIRCLE_LLA(uint8_t *buf)
void mission_init(void)
Init mission structure.
bool mission_insert(enum MissionInsertMode insert, struct _mission_element *element)
Insert a mission element according to the insertion mode.
bool mission_element_convert(struct _mission_element *el)
Convert mission element's points format if needed.
bool mission_register(mission_custom_cb cb, char *type)
Register a new navigation or action callback function.
int mission_parse_PATH_LLA(uint8_t *buf)
void mission_status_report(void)
Report mission status.
MissionInsertMode
int mission_parse_END_MISSION(uint8_t *buf)
int mission_parse_PATH(uint8_t *buf)
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
#define MISSION_REGISTER_NB
Max number of registered nav/action callbacks can be redefined.
int mission_parse_GOTO_WP_LLA(uint8_t *buf)
replace current element
int32_t alt
in millimeters above WGS84 reference ellipsoid
int mission_parse_NEXT_MISSION(uint8_t *buf)
float element_time
time in second spend in the current element
#define MISSION_ELEMENT_NB
Max number of elements in the tasks' list can be redefined.
int mission_parse_CIRCLE(uint8_t *buf)
mission planner library
add at the last position
mission_custom_cb cb
navigation/action function callback
int32_t lon
in degrees*1e7
int mission_parse_SEGMENT(uint8_t *buf)
int mission_parse_GOTO_MISSION(uint8_t *buf)
uint8_t insert_idx
inserstion index
#define MISSION_TYPE_SIZE
unsigned char uint8_t
Definition: types.h:14
uint8_t index
index of mission element
#define MISSION_PATH_NB
uint8_t current_idx
current mission element index
struct _mission mission
int mission_parse_GOTO_WP(uint8_t *buf)
Parsing functions called when a mission message is received.
struct _mission_element elements[MISSION_ELEMENT_NB]
bool 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.
int32_t lat
in degrees*1e7
int mission_parse_CUSTOM(uint8_t *buf)
static struct _mission_registered * mission_get_registered(char *type)
enum MissionType type
union _mission_element::@299 element