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