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