Paparazzi UAS  v7.0_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  return waypoints[wp_id].lla.alt/1000.f - state.ned_origin_i.lla.alt/1000.f;
125  }
126  return 0.f;
127 }
128 
130 {
131  if (wp_id < nb_waypoint) {
132  if (!waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
133  waypoint_globalize(wp_id);
134  }
135  return DEG_OF_EM7DEG(waypoints[wp_id].lla.lat);
136  }
137  else {
138  return 0.f;
139  }
140 }
141 
143 {
144  return RadOfDeg(waypoint_get_lat_deg(wp_id));
145 }
146 
148 {
149  if (wp_id < nb_waypoint) {
150  if (!waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
151  waypoint_globalize(wp_id);
152  }
153  return DEG_OF_EM7DEG(waypoints[wp_id].lla.lon);
154  }
155  else {
156  return 0.f;
157  }
158 }
159 
161 {
162  return RadOfDeg(waypoint_get_lon_deg(wp_id));
163 }
164 
165 void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
166 {
167  if (wp_id < nb_waypoint) {
168  waypoints[wp_id].enu_i = *enu;
169  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
170  ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
171  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
172  ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
173  waypoint_globalize(wp_id);
174  }
175 }
176 
177 void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
178 {
179  if (wp_id < nb_waypoint) {
180  waypoints[wp_id].enu_f = *enu;
181  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
182  ENU_BFP_OF_REAL(waypoints[wp_id].enu_i, waypoints[wp_id].enu_f);
183  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
184  ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
185  waypoint_globalize(wp_id);
186  }
187 }
188 
189 void waypoint_move_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos)
190 {
191  if (wp_id < nb_waypoint) {
192  waypoint_set_enu_i(wp_id, new_pos);
193  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x),
194  &(new_pos->y), &(new_pos->z));
195  }
196 }
197 
203 {
204  if (wp_id < nb_waypoint) {
205  waypoints[wp_id].enu_i.x = x;
206  waypoints[wp_id].enu_i.y = y;
207  /* also update ENU float representation */
208  waypoints[wp_id].enu_f.x = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.x);
209  waypoints[wp_id].enu_f.y = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.y);
210  waypoint_globalize(wp_id);
211  }
212 }
213 
215 {
216  if (wp_id < nb_waypoint) {
217  waypoint_set_xy_i(wp_id, x, y);
218  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &x, &y,
219  &(waypoints[wp_id].enu_i.z));
220  }
221 }
222 
224 {
225  if (wp_id < nb_waypoint) {
226  waypoints[wp_id].enu_i.z = alt;
227  /* also update ENU float representation */
228  waypoints[wp_id].enu_f.z = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.z);
229  waypoint_globalize(wp_id);
230  }
231 }
232 
233 void waypoint_set_alt(uint8_t wp_id, float alt)
234 {
235  if (wp_id < nb_waypoint) {
236  waypoints[wp_id].enu_f.z = alt;
237  /* also update ENU fixed point representation */
238  waypoints[wp_id].enu_i.z = POS_BFP_OF_REAL(waypoints[wp_id].enu_f.z);
239  waypoint_globalize(wp_id);
240  }
241 }
242 
243 void waypoint_set_lla(uint8_t wp_id, struct LlaCoor_i *lla)
244 {
245  if (wp_id >= nb_waypoint) {
246  return;
247  }
248  waypoints[wp_id].lla = *lla;
249  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
250  waypoint_localize(wp_id);
251 }
252 
253 void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
254 {
255  if (wp_id >= nb_waypoint) {
256  return;
257  }
258  waypoint_set_lla(wp_id, lla);
259  if (waypoint_is_global(wp_id)) {
260  /* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */
262  DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id,
263  &lla->lat, &lla->lon, &hmsl);
264  } else {
265  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
266  &waypoints[wp_id].enu_i.x,
267  &waypoints[wp_id].enu_i.y,
268  &waypoints[wp_id].enu_i.z);
269  }
270 }
271 
273 void waypoint_set_latlon(uint8_t wp_id, struct LlaCoor_i *lla)
274 {
275  if (wp_id >= nb_waypoint) {
276  return;
277  }
278  waypoints[wp_id].lla.lat = lla->lat;
279  waypoints[wp_id].lla.lon = lla->lon;
280  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
281  waypoint_localize(wp_id);
282 }
283 
286 {
287  if (wp_id >= nb_waypoint) {
288  return;
289  }
290  if (waypoint_is_global(wp_id)) {
292  } else {
294  }
295 }
296 
299 {
300  if (wp_id >= nb_waypoint) {
301  return;
302  }
303  if (waypoint_is_global(wp_id)) {
305  } else {
307  }
308 }
309 
311 {
312  if (wp_id >= nb_waypoint) {
313  return;
314  }
315  if (waypoint_is_global(wp_id)) {
316  /* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */
317  struct LlaCoor_i *lla = &(waypoints[wp_id].lla);
319  DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id,
320  &lla->lat, &lla->lon, &hmsl);
321  } else {
322  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
323  &waypoints[wp_id].enu_i.x,
324  &waypoints[wp_id].enu_i.y,
325  &waypoints[wp_id].enu_i.z);
326  }
327 }
328 
330 {
331  if (state.ned_initialized_i) {
332  struct EcefCoor_i ecef;
334  lla_of_ecef_i(&waypoints[wp_id].lla, &ecef);
335  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
336  }
337 }
338 
341 {
342  if (state.ned_initialized_i) {
343  struct EnuCoor_i enu;
345  // convert ENU pos from cm to BFP with INT32_POS_FRAC
346  enu.x = POS_BFP_OF_REAL((int64_t) enu.x) / 100;
347  enu.y = POS_BFP_OF_REAL((int64_t) enu.y) / 100;
348  enu.z = POS_BFP_OF_REAL((int64_t) enu.z) / 100;
349  waypoints[wp_id].enu_i = enu;
350  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
351  ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
352  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
353  }
354 }
355 
358 {
359  uint8_t i = 0;
360  for (i = 0; i < nb_waypoint; i++) {
361  if (waypoint_is_global(i)) {
363  }
364  }
365 }
366 
375 {
376  if (wp_id < nb_waypoint) {
377  if (!waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
378  waypoint_globalize(wp_id);
379  }
380  return &waypoints[wp_id].lla;
381  }
382  else {
383  return NULL;
384  }
385 }
386 
388 {
389  if (wp_id < nb_waypoint) {
390  if (waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_ENU_F)) {
391  waypoint_localize(wp_id);
392  }
393  return &waypoints[wp_id].enu_f;
394  }
395  else {
396  return NULL;
397  }
398 }
399 
401 {
402  if (wp_id < nb_waypoint) {
403  if (waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_ENU_I)) {
404  waypoint_localize(wp_id);
405  }
406  return &waypoints[wp_id].enu_i;
407  }
408  else {
409  return NULL;
410  }
411 }
412 
413 void waypoint_copy(uint8_t wp_dest, uint8_t wp_src)
414 {
415  if (wp_dest < nb_waypoint && wp_src < nb_waypoint) {
416  waypoints[wp_dest] = waypoints[wp_src];
417  }
418 }
419 
421 {
422  if (wp_dest < nb_waypoint && wp_src < nb_waypoint) {
423  waypoints[wp_dest].enu_f.x = waypoints[wp_src].enu_f.x;
424  waypoints[wp_dest].enu_f.y = waypoints[wp_src].enu_f.y;
425  waypoints[wp_dest].enu_i.x = waypoints[wp_src].enu_i.x;
426  waypoints[wp_dest].enu_i.y = waypoints[wp_src].enu_i.y;
427  waypoints[wp_dest].lla.lat = waypoints[wp_src].lla.lat;
428  waypoints[wp_dest].lla.lon = waypoints[wp_src].lla.lon;
429  }
430 }
#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:413
void waypoint_globalize(uint8_t wp_id)
update global LLA coordinates from its ENU coordinates
Definition: waypoints.c:329
void waypoint_set_here_2d(uint8_t wp_id)
set waypoint to current horizontal location without modifying altitude
Definition: waypoints.c:298
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:357
struct LlaCoor_i * waypoint_get_lla(uint8_t wp_id)
Get LLA coordinates of waypoint.
Definition: waypoints.c:374
void waypoint_position_copy(uint8_t wp_dest, uint8_t wp_src)
Definition: waypoints.c:420
void waypoint_move_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Definition: waypoints.c:214
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:160
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:202
void waypoint_set_here(uint8_t wp_id)
set waypoint to current location and altitude
Definition: waypoints.c:285
float waypoint_get_lon_deg(uint8_t wp_id)
Get longitude of waypoint in deg.
Definition: waypoints.c:147
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:273
void waypoints_init(void)
initialize global and local waypoints
Definition: waypoints.c:51
float waypoint_get_lla_alt(uint8_t wp_id)
Definition: waypoints.c:121
float waypoint_get_lat_deg(uint8_t wp_id)
Get latitude of waypoint in deg.
Definition: waypoints.c:129
void waypoint_move_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos)
Definition: waypoints.c:189
struct EnuCoor_i * waypoint_get_enu_i(uint8_t wp_id)
Get ENU coordinates (integer)
Definition: waypoints.c:400
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:165
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:243
void waypoint_set_alt_i(uint8_t wp_id, int32_t alt)
Definition: waypoints.c:223
void waypoint_move_here_2d(uint8_t wp_id)
Definition: waypoints.c:310
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:340
void waypoint_set_alt(uint8_t wp_id, float alt)
Set altitude of waypoint in meters (above reference)
Definition: waypoints.c:233
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:387
void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
Definition: waypoints.c:253
float waypoint_get_lat_rad(uint8_t wp_id)
Get latitude of waypoint in rad.
Definition: waypoints.c:142
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition: waypoints.c:177
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