Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
waypoints.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
26 #include "modules/nav/waypoints.h"
27 #include "state.h"
29 #include "generated/flight_plan.h"
30 
31 const uint8_t nb_waypoint = NB_WAYPOINT;
32 struct Waypoint waypoints[NB_WAYPOINT];
33 
34 #if PERIODIC_TELEMETRY
36 
37 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
38 {
39  static uint8_t i;
40  i++;
41  if (i >= nb_waypoint) { i = 0; }
42  pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
43  &i,
44  &(waypoints[i].enu_i.x),
45  &(waypoints[i].enu_i.y),
46  &(waypoints[i].enu_i.z));
47 }
48 #endif
49 
51 void waypoints_init(void)
52 {
53  struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU;
54  struct LlaCoor_i wp_tmp_lla_i[NB_WAYPOINT] = WAYPOINTS_LLA_WGS84;
55  /* element in array is TRUE if absolute/global waypoint */
56  bool is_global[NB_WAYPOINT] = WAYPOINTS_GLOBAL;
57  uint8_t i = 0;
58  for (i = 0; i < nb_waypoint; i++) {
59  /* clear all flags */
60  waypoints[i].flags = 0;
61  /* init waypoint as global LLA or local ENU */
62  if (is_global[i]) {
64  waypoint_set_lla(i, &wp_tmp_lla_i[i]);
65  } else {
66  waypoint_set_enu(i, &wp_tmp_float[i]);
67  }
68  }
69 
70 #if PERIODIC_TELEMETRY
72 #endif
73 }
74 
76 {
77  if (wp_id < nb_waypoint) {
78  return bit_is_set(waypoints[wp_id].flags, WP_FLAG_GLOBAL);
79  }
80  return false;
81 }
82 
84 {
85  if (wp_id < nb_waypoint) {
86  SetBit(waypoints[wp_id].flags, WP_FLAG_GLOBAL);
87  }
88 }
89 
91 {
92  if (wp_id < nb_waypoint) {
93  ClearBit(waypoints[wp_id].flags, WP_FLAG_GLOBAL);
94  }
95 }
96 
97 float waypoint_get_x(uint8_t wp_id)
98 {
99  if (wp_id < nb_waypoint) {
100  return waypoints[wp_id].enu_f.x;
101  }
102  return 0.f;
103 }
104 
106 {
107  if (wp_id < nb_waypoint) {
108  return waypoints[wp_id].enu_f.y;
109  }
110  return 0.f;
111 }
112 
114 {
115  if (wp_id < nb_waypoint) {
116  return waypoints[wp_id].enu_f.z;
117  }
118  return 0.f;
119 }
120 
122 {
123  if (wp_id < nb_waypoint) {
124  if (!waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
125  waypoint_globalize(wp_id);
126  }
127  return DEG_OF_EM7DEG(waypoints[wp_id].lla.lat);
128  }
129  else {
130  return 0.f;
131  }
132 }
133 
135 {
136  return RadOfDeg(waypoint_get_lat_deg(wp_id));
137 }
138 
140 {
141  if (wp_id < nb_waypoint) {
142  if (!waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
143  waypoint_globalize(wp_id);
144  }
145  return DEG_OF_EM7DEG(waypoints[wp_id].lla.lon);
146  }
147  else {
148  return 0.f;
149  }
150 }
151 
153 {
154  return RadOfDeg(waypoint_get_lon_deg(wp_id));
155 }
156 
157 void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
158 {
159  if (wp_id < nb_waypoint) {
160  waypoints[wp_id].enu_i = *enu;
161  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
162  ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
163  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
164  ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
165  waypoint_globalize(wp_id);
166  }
167 }
168 
169 void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
170 {
171  if (wp_id < nb_waypoint) {
172  waypoints[wp_id].enu_f = *enu;
173  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
174  ENU_BFP_OF_REAL(waypoints[wp_id].enu_i, waypoints[wp_id].enu_f);
175  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
176  ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
177  waypoint_globalize(wp_id);
178  }
179 }
180 
181 void waypoint_move_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos)
182 {
183  if (wp_id < nb_waypoint) {
184  waypoint_set_enu_i(wp_id, new_pos);
185  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x),
186  &(new_pos->y), &(new_pos->z));
187  }
188 }
189 
195 {
196  if (wp_id < nb_waypoint) {
197  waypoints[wp_id].enu_i.x = x;
198  waypoints[wp_id].enu_i.y = y;
199  /* also update ENU float representation */
200  waypoints[wp_id].enu_f.x = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.x);
201  waypoints[wp_id].enu_f.y = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.y);
202  waypoint_globalize(wp_id);
203  }
204 }
205 
207 {
208  if (wp_id < nb_waypoint) {
209  waypoint_set_xy_i(wp_id, x, y);
210  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &x, &y,
211  &(waypoints[wp_id].enu_i.z));
212  }
213 }
214 
216 {
217  if (wp_id < nb_waypoint) {
218  waypoints[wp_id].enu_i.z = alt;
219  /* also update ENU float representation */
220  waypoints[wp_id].enu_f.z = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.z);
221  waypoint_globalize(wp_id);
222  }
223 }
224 
225 void waypoint_set_alt(uint8_t wp_id, float alt)
226 {
227  if (wp_id < nb_waypoint) {
228  waypoints[wp_id].enu_f.z = alt;
229  /* also update ENU fixed point representation */
230  waypoints[wp_id].enu_i.z = POS_BFP_OF_REAL(waypoints[wp_id].enu_f.z);
231  waypoint_globalize(wp_id);
232  }
233 }
234 
235 void waypoint_set_lla(uint8_t wp_id, struct LlaCoor_i *lla)
236 {
237  if (wp_id >= nb_waypoint) {
238  return;
239  }
240  waypoints[wp_id].lla = *lla;
241  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
242  waypoint_localize(wp_id);
243 }
244 
245 void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
246 {
247  if (wp_id >= nb_waypoint) {
248  return;
249  }
250  waypoint_set_lla(wp_id, lla);
251  if (waypoint_is_global(wp_id)) {
252  /* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */
254  DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id,
255  &lla->lat, &lla->lon, &hmsl);
256  } else {
257  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
258  &waypoints[wp_id].enu_i.x,
259  &waypoints[wp_id].enu_i.y,
260  &waypoints[wp_id].enu_i.z);
261  }
262 }
263 
265 void waypoint_set_latlon(uint8_t wp_id, struct LlaCoor_i *lla)
266 {
267  if (wp_id >= nb_waypoint) {
268  return;
269  }
270  waypoints[wp_id].lla.lat = lla->lat;
271  waypoints[wp_id].lla.lon = lla->lon;
272  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
273  waypoint_localize(wp_id);
274 }
275 
278 {
279  if (wp_id >= nb_waypoint) {
280  return;
281  }
282  if (waypoint_is_global(wp_id)) {
284  } else {
286  }
287 }
288 
291 {
292  if (wp_id >= nb_waypoint) {
293  return;
294  }
295  if (waypoint_is_global(wp_id)) {
297  } else {
299  }
300 }
301 
303 {
304  if (wp_id >= nb_waypoint) {
305  return;
306  }
307  if (waypoint_is_global(wp_id)) {
308  /* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */
309  struct LlaCoor_i *lla = &(waypoints[wp_id].lla);
311  DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id,
312  &lla->lat, &lla->lon, &hmsl);
313  } else {
314  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
315  &waypoints[wp_id].enu_i.x,
316  &waypoints[wp_id].enu_i.y,
317  &waypoints[wp_id].enu_i.z);
318  }
319 }
320 
322 {
323  if (state.ned_initialized_i) {
324  struct EcefCoor_i ecef;
326  lla_of_ecef_i(&waypoints[wp_id].lla, &ecef);
327  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
328  }
329 }
330 
333 {
334  if (state.ned_initialized_i) {
335  struct EnuCoor_i enu;
337  // convert ENU pos from cm to BFP with INT32_POS_FRAC
338  enu.x = POS_BFP_OF_REAL(enu.x) / 100;
339  enu.y = POS_BFP_OF_REAL(enu.y) / 100;
340  enu.z = POS_BFP_OF_REAL(enu.z) / 100;
341  waypoints[wp_id].enu_i = enu;
342  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
343  ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
344  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
345  }
346 }
347 
350 {
351  uint8_t i = 0;
352  for (i = 0; i < nb_waypoint; i++) {
353  if (waypoint_is_global(i)) {
355  }
356  }
357 }
358 
367 {
368  if (wp_id < nb_waypoint) {
369  if (!waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
370  waypoint_globalize(wp_id);
371  }
372  return &waypoints[wp_id].lla;
373  }
374  else {
375  return NULL;
376  }
377 }
378 
380 {
381  if (wp_id < nb_waypoint) {
382  if (waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_ENU_F)) {
383  waypoint_localize(wp_id);
384  }
385  return &waypoints[wp_id].enu_f;
386  }
387  else {
388  return NULL;
389  }
390 }
391 
393 {
394  if (wp_id < nb_waypoint) {
395  if (waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_ENU_I)) {
396  waypoint_localize(wp_id);
397  }
398  return &waypoints[wp_id].enu_i;
399  }
400  else {
401  return NULL;
402  }
403 }
404 
405 void waypoint_copy(uint8_t wp_dest, uint8_t wp_src)
406 {
407  if (wp_dest < nb_waypoint && wp_src < nb_waypoint) {
408  waypoints[wp_dest] = waypoints[wp_src];
409  }
410 }
411 
413 {
414  if (wp_dest < nb_waypoint && wp_src < nb_waypoint) {
415  waypoints[wp_dest].enu_f.x = waypoints[wp_src].enu_f.x;
416  waypoints[wp_dest].enu_f.y = waypoints[wp_src].enu_f.y;
417  waypoints[wp_dest].enu_i.x = waypoints[wp_src].enu_i.x;
418  waypoints[wp_dest].enu_i.y = waypoints[wp_src].enu_i.y;
419  waypoints[wp_dest].lla.lat = waypoints[wp_src].lla.lat;
420  waypoints[wp_dest].lla.lon = waypoints[wp_src].lla.lon;
421  }
422 }
#define POS_FLOAT_OF_BFP(_ai)
#define POS_BFP_OF_REAL(_af)
int32_t lat
in degrees*1e7
int32_t y
North.
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Up.
struct LlaCoor_i lla
Reference point in lla.
int32_t x
East.
int32_t lon
in degrees*1e7
#define ENU_FLOAT_OF_BFP(_o, _i)
#define ENU_BFP_OF_REAL(_o, _i)
#define DEG_OF_EM7DEG(_r)
void ecef_of_enu_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Convert a local ENU position to ECEF.
void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local ENU.
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
struct State state
Definition: state.c:36
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
void waypoint_copy(uint8_t wp_dest, uint8_t wp_src)
copy one waypoint to another, this includes all flags from the source waypoint
Definition: waypoints.c:405
void waypoint_globalize(uint8_t wp_id)
update global LLA coordinates from its ENU coordinates
Definition: waypoints.c:321
void waypoint_set_here_2d(uint8_t wp_id)
set waypoint to current horizontal location without modifying altitude
Definition: waypoints.c:290
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:349
struct LlaCoor_i * waypoint_get_lla(uint8_t wp_id)
Get LLA coordinates of waypoint.
Definition: waypoints.c:366
void waypoint_position_copy(uint8_t wp_dest, uint8_t wp_src)
Definition: waypoints.c:412
void waypoint_move_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Definition: waypoints.c:206
bool waypoint_is_global(uint8_t wp_id)
Definition: waypoints.c:75
float waypoint_get_lon_rad(uint8_t wp_id)
Get longitude of waypoint in rad.
Definition: waypoints.c:152
float waypoint_get_alt(uint8_t wp_id)
Get altitude of waypoint in meters (above reference)
Definition: waypoints.c:113
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition: waypoints.c:97
void waypoint_set_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Set only local XY coordinates of waypoint without update altitude.
Definition: waypoints.c:194
void waypoint_set_here(uint8_t wp_id)
set waypoint to current location and altitude
Definition: waypoints.c:277
float waypoint_get_lon_deg(uint8_t wp_id)
Get longitude of waypoint in deg.
Definition: waypoints.c:139
void waypoint_set_global_flag(uint8_t wp_id)
Definition: waypoints.c:83
void waypoint_set_latlon(uint8_t wp_id, struct LlaCoor_i *lla)
set waypoint latitude/longitude without updating altitude
Definition: waypoints.c:265
void waypoints_init(void)
initialize global and local waypoints
Definition: waypoints.c:51
float waypoint_get_lat_deg(uint8_t wp_id)
Get latitude of waypoint in deg.
Definition: waypoints.c:121
void waypoint_move_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos)
Definition: waypoints.c:181
struct EnuCoor_i * waypoint_get_enu_i(uint8_t wp_id)
Get ENU coordinates (integer)
Definition: waypoints.c:392
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition: waypoints.c:105
const uint8_t nb_waypoint
Definition: waypoints.c:31
void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
Definition: waypoints.c:157
void waypoint_clear_global_flag(uint8_t wp_id)
Definition: waypoints.c:90
void waypoint_set_lla(uint8_t wp_id, struct LlaCoor_i *lla)
Definition: waypoints.c:235
void waypoint_set_alt_i(uint8_t wp_id, int32_t alt)
Definition: waypoints.c:215
void waypoint_move_here_2d(uint8_t wp_id)
Definition: waypoints.c:302
struct Waypoint waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: waypoints.c:32
void waypoint_localize(uint8_t wp_id)
update local ENU coordinates from its LLA coordinates
Definition: waypoints.c:332
void waypoint_set_alt(uint8_t wp_id, float alt)
Set altitude of waypoint in meters (above reference)
Definition: waypoints.c:225
static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
Definition: waypoints.c:37
struct EnuCoor_f * waypoint_get_enu_f(uint8_t wp_id)
Get ENU coordinates (float)
Definition: waypoints.c:379
void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
Definition: waypoints.c:245
float waypoint_get_lat_rad(uint8_t wp_id)
Get latitude of waypoint in rad.
Definition: waypoints.c:134
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition: waypoints.c:169
struct LlaCoor_i lla
Definition: waypoints.h:46
#define WP_FLAG_ENU_I
Definition: waypoints.h:38
#define WP_FLAG_GLOBAL
Definition: waypoints.h:37
uint8_t flags
bitmask encoding valid representations and if local or global
Definition: waypoints.h:43
struct EnuCoor_i enu_i
with INT32_POS_FRAC
Definition: waypoints.h:44
struct EnuCoor_f enu_f
Definition: waypoints.h:45
#define WP_FLAG_ENU_F
Definition: waypoints.h:39
#define WP_FLAG_LLA_I
Definition: waypoints.h:40
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
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
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98