Paparazzi UAS  v7.0_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 #if PERIODIC_TELEMETRY
36 #endif
37 
38 // Check for unique ID by default
39 #ifndef MISSION_CHECK_UNIQUE_ID
40 #define MISSION_CHECK_UNIQUE_ID TRUE
41 #endif
42 
43 struct _mission mission = { 0 };
44 
45 static void send_mission_status(struct transport_tx *trans, struct link_device *device)
46 {
47  // build index list
48  uint8_t index_list[MISSION_ELEMENT_NB];
49  uint8_t i = mission.current_idx, j = 0;
50  while (i != mission.insert_idx) {
51  index_list[j++] = mission.elements[i].index;
52  i = (i + 1) % MISSION_ELEMENT_NB;
53  }
54  if (j == 0) { index_list[j++] = 0; } // Dummy value if index list is empty
55  //compute remaining time (or -1. if no time limit)
56  float remaining_time = -1.;
59  }
60 
61  // send status
62  pprz_msg_send_MISSION_STATUS(trans, device, AC_ID, &remaining_time, j, index_list);
63 }
64 
65 void mission_init(void)
66 {
67  mission.insert_idx = 0;
68  mission.current_idx = 0;
69  mission.element_time = 0.;
70 
71  for (int i = 0; i < MISSION_REGISTER_NB; i++) {
72  mission.registered[i].cb = NULL;
73  memset(mission.registered[i].type, '\0', MISSION_TYPE_SIZE);
74  }
75 
76 #if PERIODIC_TELEMETRY
78 #endif
79 }
80 
81 
82 // Insert element
83 bool mission_insert(enum MissionInsertMode insert, struct _mission_element *element)
84 {
85  uint8_t tmp;
86 
87 #if MISSION_CHECK_UNIQUE_ID
88  // check that the new element id is not already in the list
89  if (mission_get_from_index(element->index) != NULL) {
90  return false;
91  }
92 #endif
93 
94  switch (insert) {
95  case Append:
97  if (tmp == mission.current_idx) { return false; } // no room to insert element
98  mission.elements[mission.insert_idx] = *element; // add element
99  mission.insert_idx = tmp; // move insert index
100  break;
101  case Prepend:
102  if (mission.current_idx == 0) { tmp = MISSION_ELEMENT_NB - 1; }
103  else { tmp = mission.current_idx - 1; }
104  if (tmp == mission.insert_idx) { return false; } // no room to inser element
105  mission.elements[tmp] = *element; // add element
106  mission.current_idx = tmp; // move current index
107  mission.element_time = 0.; // reset timer
108  break;
109  case ReplaceCurrent:
110  // current element can always be modified, index are not changed
111  mission.elements[mission.current_idx] = *element;
112  mission.element_time = 0.; // reset timer
113  break;
114  case ReplaceAll:
115  // reset queue and index
116  mission.elements[0] = *element;
117  mission.current_idx = 0;
118  mission.insert_idx = 1;
119  mission.element_time = 0.; // reset timer
120  break;
121  case ReplaceNexts:
122  tmp = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
123  mission.elements[tmp] = *element;
125  break;
126  default:
127  // unknown insertion mode
128  return false;
129  }
130  return true;
131 
132 }
133 
134 // Register new callback
136 {
137  for (int i = 0; i < MISSION_REGISTER_NB; i++) {
138  if (str_equal(mission.registered[i].type, type)) {
139  return false; // identifier already registered
140  }
141  if (mission.registered[i].cb == NULL) {
142  strncpy(mission.registered[i].type, type, MISSION_TYPE_SIZE-1);
143  mission.registered[i].cb = cb;
144  return true;
145  }
146  }
147  return false; // no more room to register callbacks
148 }
149 
150 // Returns a pointer to a register struct with matching types, NULL if not found
152 {
153  for (int i = 0; i < MISSION_REGISTER_NB; i++) {
154  if (str_equal(mission.registered[i].type, type)) {
155  return &(mission.registered[i]);
156  }
157  }
158  return NULL; // not found
159 }
160 
161 // Returns a pointer to a mission element struct with matching index, NULL if not found
163 {
165  while (i != mission.insert_idx) {
166  if (mission.elements[i].index == index) {
167  return &mission.elements[i]; // return first next element with matching index
168  }
169  i++;
170  if (i == MISSION_ELEMENT_NB) {
171  i = 0;
172  }
173  }
174  return NULL; // not found
175 }
176 
177 // Get element
179 {
181  return NULL;
182  }
183  return &(mission.elements[mission.current_idx]);
184 }
185 
186 
187 // Report function
189 {
190  send_mission_status(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
191 }
192 
193 
195 // Parsing functions //
197 
199 {
200  if (DL_MISSION_GOTO_WP_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
201 
202  struct _mission_element me;
203  me.type = MissionWP;
204  me.element.mission_wp.wp.x = DL_MISSION_GOTO_WP_wp_east(buf);
205  me.element.mission_wp.wp.y = DL_MISSION_GOTO_WP_wp_north(buf);
206  me.element.mission_wp.wp.z = DL_MISSION_GOTO_WP_wp_alt(buf);
207  me.duration = DL_MISSION_GOTO_WP_duration(buf);
208  me.index = DL_MISSION_GOTO_WP_index(buf);
209 
210  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_insert(buf));
211 
212  return mission_insert(insert, &me);
213 }
214 
216 {
217  if (DL_MISSION_GOTO_WP_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
218 
219  struct LlaCoor_i lla;
220  lla.lat = DL_MISSION_GOTO_WP_LLA_wp_lat(buf);
221  lla.lon = DL_MISSION_GOTO_WP_LLA_wp_lon(buf);
222  lla.alt = DL_MISSION_GOTO_WP_LLA_wp_alt(buf);
223 
224  struct _mission_element me;
225  me.type = MissionWP;
226  // if there is no valid local coordinate, do not insert mission element
227  if (!mission_point_of_lla(&me.element.mission_wp.wp, &lla)) { return false; }
228  me.duration = DL_MISSION_GOTO_WP_LLA_duration(buf);
229  me.index = DL_MISSION_GOTO_WP_LLA_index(buf);
230 
231  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_LLA_insert(buf));
232 
233  return mission_insert(insert, &me);
234 }
235 
237 {
238  if (DL_MISSION_CIRCLE_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
239 
240  struct _mission_element me;
241  me.type = MissionCircle;
242  me.element.mission_circle.center.x = DL_MISSION_CIRCLE_center_east(buf);
243  me.element.mission_circle.center.y = DL_MISSION_CIRCLE_center_north(buf);
244  me.element.mission_circle.center.z = DL_MISSION_CIRCLE_center_alt(buf);
245  me.element.mission_circle.radius = DL_MISSION_CIRCLE_radius(buf);
246  me.duration = DL_MISSION_CIRCLE_duration(buf);
247  me.index = DL_MISSION_CIRCLE_index(buf);
248 
249  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_insert(buf));
250 
251  return mission_insert(insert, &me);
252 }
253 
255 {
256  if (DL_MISSION_CIRCLE_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
257 
258  struct LlaCoor_i lla;
259  lla.lat = DL_MISSION_CIRCLE_LLA_center_lat(buf);
260  lla.lon = DL_MISSION_CIRCLE_LLA_center_lon(buf);
261  lla.alt = DL_MISSION_CIRCLE_LLA_center_alt(buf);
262 
263  struct _mission_element me;
264  me.type = MissionCircle;
265  // if there is no valid local coordinate, do not insert mission element
266  if (!mission_point_of_lla(&me.element.mission_circle.center, &lla)) { return false; }
267  me.element.mission_circle.radius = DL_MISSION_CIRCLE_LLA_radius(buf);
268  me.duration = DL_MISSION_CIRCLE_LLA_duration(buf);
269  me.index = DL_MISSION_CIRCLE_LLA_index(buf);
270 
271  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_LLA_insert(buf));
272 
273  return mission_insert(insert, &me);
274 }
275 
277 {
278  if (DL_MISSION_SEGMENT_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
279 
280  struct _mission_element me;
281  me.type = MissionSegment;
282  me.element.mission_segment.from.x = DL_MISSION_SEGMENT_segment_east_1(buf);
283  me.element.mission_segment.from.y = DL_MISSION_SEGMENT_segment_north_1(buf);
284  me.element.mission_segment.from.z = DL_MISSION_SEGMENT_segment_alt(buf);
285  me.element.mission_segment.to.x = DL_MISSION_SEGMENT_segment_east_2(buf);
286  me.element.mission_segment.to.y = DL_MISSION_SEGMENT_segment_north_2(buf);
287  me.element.mission_segment.to.z = DL_MISSION_SEGMENT_segment_alt(buf);
288  me.duration = DL_MISSION_SEGMENT_duration(buf);
289  me.index = DL_MISSION_SEGMENT_index(buf);
290 
291  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_insert(buf));
292 
293  return mission_insert(insert, &me);
294 }
295 
297 {
298  if (DL_MISSION_SEGMENT_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
299 
300  struct LlaCoor_i from_lla, to_lla;
301  from_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_1(buf);
302  from_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_1(buf);
303  from_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(buf);
304  to_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_2(buf);
305  to_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_2(buf);
306  to_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(buf);
307 
308  struct _mission_element me;
309  me.type = MissionSegment;
310  // if there is no valid local coordinate, do not insert mission element
311  if (!mission_point_of_lla(&me.element.mission_segment.from, &from_lla)) { return false; }
312  if (!mission_point_of_lla(&me.element.mission_segment.to, &to_lla)) { return false; }
313  me.duration = DL_MISSION_SEGMENT_LLA_duration(buf);
314  me.index = DL_MISSION_SEGMENT_LLA_index(buf);
315 
316  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_LLA_insert(buf));
317 
318  return mission_insert(insert, &me);
319 }
320 
322 {
323  if (DL_MISSION_PATH_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
324 
325  struct _mission_element me;
326  me.type = MissionPath;
327  me.element.mission_path.path[0].x = DL_MISSION_PATH_point_east_1(buf);
328  me.element.mission_path.path[0].y = DL_MISSION_PATH_point_north_1(buf);
329  me.element.mission_path.path[0].z = DL_MISSION_PATH_path_alt(buf);
330  me.element.mission_path.path[1].x = DL_MISSION_PATH_point_east_2(buf);
331  me.element.mission_path.path[1].y = DL_MISSION_PATH_point_north_2(buf);
332  me.element.mission_path.path[1].z = DL_MISSION_PATH_path_alt(buf);
333  me.element.mission_path.path[2].x = DL_MISSION_PATH_point_east_3(buf);
334  me.element.mission_path.path[2].y = DL_MISSION_PATH_point_north_3(buf);
335  me.element.mission_path.path[2].z = DL_MISSION_PATH_path_alt(buf);
336  me.element.mission_path.path[3].x = DL_MISSION_PATH_point_east_4(buf);
337  me.element.mission_path.path[3].y = DL_MISSION_PATH_point_north_4(buf);
338  me.element.mission_path.path[3].z = DL_MISSION_PATH_path_alt(buf);
339  me.element.mission_path.path[4].x = DL_MISSION_PATH_point_east_5(buf);
340  me.element.mission_path.path[4].y = DL_MISSION_PATH_point_north_5(buf);
341  me.element.mission_path.path[4].z = DL_MISSION_PATH_path_alt(buf);
342  me.element.mission_path.nb = DL_MISSION_PATH_nb(buf);
343  if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; }
344  me.element.mission_path.path_idx = 0;
345  me.duration = DL_MISSION_PATH_duration(buf);
346  me.index = DL_MISSION_PATH_index(buf);
347 
348  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_insert(buf));
349 
350  return mission_insert(insert, &me);
351 }
352 
354 {
355  if (DL_MISSION_PATH_LLA_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
356 
357  struct LlaCoor_i lla[MISSION_PATH_NB];
358  lla[0].lat = DL_MISSION_PATH_LLA_point_lat_1(buf);
359  lla[0].lon = DL_MISSION_PATH_LLA_point_lon_1(buf);
360  lla[0].alt = DL_MISSION_PATH_LLA_path_alt(buf);
361  lla[1].lat = DL_MISSION_PATH_LLA_point_lat_2(buf);
362  lla[1].lon = DL_MISSION_PATH_LLA_point_lon_2(buf);
363  lla[1].alt = DL_MISSION_PATH_LLA_path_alt(buf);
364  lla[2].lat = DL_MISSION_PATH_LLA_point_lat_3(buf);
365  lla[2].lon = DL_MISSION_PATH_LLA_point_lon_3(buf);
366  lla[2].alt = DL_MISSION_PATH_LLA_path_alt(buf);
367  lla[3].lat = DL_MISSION_PATH_LLA_point_lat_4(buf);
368  lla[3].lon = DL_MISSION_PATH_LLA_point_lon_4(buf);
369  lla[3].alt = DL_MISSION_PATH_LLA_path_alt(buf);
370  lla[4].lat = DL_MISSION_PATH_LLA_point_lat_5(buf);
371  lla[4].lon = DL_MISSION_PATH_LLA_point_lon_5(buf);
372  lla[4].alt = DL_MISSION_PATH_LLA_path_alt(buf);
373 
374  struct _mission_element me;
375  me.type = MissionPath;
376  uint8_t i;
377  me.element.mission_path.nb = DL_MISSION_PATH_LLA_nb(buf);
378  if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; }
379  for (i = 0; i < me.element.mission_path.nb; i++) {
380  // if there is no valid local coordinate, do not insert mission element
381  if (!mission_point_of_lla(&me.element.mission_path.path[i], &lla[i])) { return false; }
382  }
383  me.element.mission_path.path_idx = 0;
384  me.duration = DL_MISSION_PATH_LLA_duration(buf);
385  me.index = DL_MISSION_PATH_LLA_index(buf);
386 
387  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_LLA_insert(buf));
388 
389  return mission_insert(insert, &me);
390 }
391 
393 {
394  if (DL_MISSION_CUSTOM_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
395 
396  struct _mission_element me;
397  me.type = MissionCustom;
398  me.element.mission_custom.reg = mission_get_registered(DL_MISSION_CUSTOM_type(buf));
399  if (me.element.mission_custom.reg == NULL) {
400  return false; // unknown type
401  }
402  me.element.mission_custom.nb = DL_MISSION_CUSTOM_params_length(buf);
403  for (int i = 0; i < me.element.mission_custom.nb; i++) {
404  me.element.mission_custom.params[i] = DL_MISSION_CUSTOM_params(buf)[i];
405  }
406  me.duration = DL_MISSION_CUSTOM_duration(buf);
407  me.index = DL_MISSION_CUSTOM_index(buf);
408 
409  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CUSTOM_insert(buf));
410 
411  return mission_insert(insert, &me);
412 }
413 
415 {
416  if (DL_MISSION_UPDATE_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
417 
418  struct _mission_element *me = mission_get_from_index(DL_MISSION_UPDATE_index(buf));
419  if (me == NULL) {
420  return false; // unknown type
421  }
422 
423  float duration = DL_MISSION_UPDATE_duration(buf);
424  if (duration > -2.f) { // no update should in fact be -9
425  me->duration = duration; // update
426  }
427 
428  uint8_t nb = DL_MISSION_UPDATE_params_length(buf);
429  float params[MISSION_CUSTOM_MAX];
430  memcpy(params, DL_MISSION_UPDATE_params(buf), nb*sizeof(float));
431 
432  switch (me->type) {
433  case MissionCustom:
434  return me->element.mission_custom.reg->cb(nb, params, MissionUpdate);
435  case MissionWP:
436  case MissionCircle:
437  case MissionSegment:
438  case MissionPath:
439  default:
440  // TODO handle update param for standard patterns
441  break;
442  }
443  return true;
444 }
445 
447 {
448  if (DL_GOTO_MISSION_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
449 
450  uint8_t mission_id = DL_GOTO_MISSION_mission_id(buf);
451  if (mission_id < MISSION_ELEMENT_NB) {
452  // reset timer
453  mission.element_time = 0.;
454  // set current index
455  mission.current_idx = mission_id;
456  } else { return false; }
457 
458  return true;
459 }
460 
462 {
463  if (DL_NEXT_MISSION_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
464 
465  if (mission.current_idx == mission.insert_idx) { return false; } // already at the last position
466 
467  // reset timer
468  mission.element_time = 0.;
469  // increment current index
471  return true;
472 }
473 
475 {
476  if (DL_END_MISSION_ac_id(buf) != AC_ID) { return false; } // not for this aircraft
477 
478  // reset timer
479  mission.element_time = 0.;
480  // set current index to insert index (last position)
482  return true;
483 }
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)
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)
static void send_mission_status(struct transport_tx *trans, struct link_device *device)
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
union _mission_element::@299 element
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.
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
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98