Paparazzi UAS  v5.15_devel-99-g2ff7410
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
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 
35 void waypoints_init(void)
36 {
37  struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU;
38  struct LlaCoor_i wp_tmp_lla_i[NB_WAYPOINT] = WAYPOINTS_LLA_WGS84;
39  /* element in array is TRUE if absolute/global waypoint */
40  bool is_global[NB_WAYPOINT] = WAYPOINTS_GLOBAL;
41  uint8_t i = 0;
42  for (i = 0; i < nb_waypoint; i++) {
43  /* clear all flags */
44  waypoints[i].flags = 0;
45  /* init waypoint as global LLA or local ENU */
46  if (is_global[i]) {
48  waypoint_set_lla(i, &wp_tmp_lla_i[i]);
49  } else {
50  waypoint_set_enu(i, &wp_tmp_float[i]);
51  }
52  }
53 }
54 
56 {
57  if (wp_id < nb_waypoint) {
58  return bit_is_set(waypoints[wp_id].flags, WP_FLAG_GLOBAL);
59  }
60  return false;
61 }
62 
64 {
65  if (wp_id < nb_waypoint) {
66  SetBit(waypoints[wp_id].flags, WP_FLAG_GLOBAL);
67  }
68 }
69 
71 {
72  if (wp_id < nb_waypoint) {
73  ClearBit(waypoints[wp_id].flags, WP_FLAG_GLOBAL);
74  }
75 }
76 
77 float waypoint_get_x(uint8_t wp_id)
78 {
79  if (wp_id < nb_waypoint) {
80  return waypoints[wp_id].enu_f.x;
81  }
82  return 0.f;
83 }
84 
85 float waypoint_get_y(uint8_t wp_id)
86 {
87  if (wp_id < nb_waypoint) {
88  return waypoints[wp_id].enu_f.y;
89  }
90  return 0.f;
91 }
92 
94 {
95  if (wp_id < nb_waypoint) {
96  return waypoints[wp_id].enu_f.z;
97  }
98  return 0.f;
99 }
100 
101 void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
102 {
103  if (wp_id < nb_waypoint) {
104  waypoints[wp_id].enu_i = *enu;
105  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
106  ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
107  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
108  ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
109  waypoint_globalize(wp_id);
110  }
111 }
112 
113 void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
114 {
115  if (wp_id < nb_waypoint) {
116  waypoints[wp_id].enu_f = *enu;
117  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
118  ENU_BFP_OF_REAL(waypoints[wp_id].enu_i, waypoints[wp_id].enu_f);
119  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
120  ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
121  waypoint_globalize(wp_id);
122  }
123 }
124 
125 void waypoint_move_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos)
126 {
127  if (wp_id < nb_waypoint) {
128  waypoint_set_enu_i(wp_id, new_pos);
129  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x),
130  &(new_pos->y), &(new_pos->z));
131  }
132 }
133 
139 {
140  if (wp_id < nb_waypoint) {
141  waypoints[wp_id].enu_i.x = x;
142  waypoints[wp_id].enu_i.y = y;
143  /* also update ENU float representation */
144  waypoints[wp_id].enu_f.x = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.x);
145  waypoints[wp_id].enu_f.y = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.y);
146  waypoint_globalize(wp_id);
147  }
148 }
149 
151 {
152  if (wp_id < nb_waypoint) {
153  waypoint_set_xy_i(wp_id, x, y);
154  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &x, &y,
155  &(waypoints[wp_id].enu_i.z));
156  }
157 }
158 
160 {
161  if (wp_id < nb_waypoint) {
162  waypoints[wp_id].enu_i.z = alt;
163  /* also update ENU float representation */
164  waypoints[wp_id].enu_f.z = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.z);
165  waypoint_globalize(wp_id);
166  }
167 }
168 
169 void waypoint_set_alt(uint8_t wp_id, float alt)
170 {
171  if (wp_id < nb_waypoint) {
172  waypoints[wp_id].enu_f.z = alt;
173  /* also update ENU fixed point representation */
174  waypoints[wp_id].enu_i.z = POS_BFP_OF_REAL(waypoints[wp_id].enu_f.z);
175  waypoint_globalize(wp_id);
176  }
177 }
178 
179 void waypoint_set_lla(uint8_t wp_id, struct LlaCoor_i *lla)
180 {
181  if (wp_id >= nb_waypoint) {
182  return;
183  }
184  waypoints[wp_id].lla = *lla;
185  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
186  waypoint_localize(wp_id);
187 }
188 
189 void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
190 {
191  if (wp_id >= nb_waypoint) {
192  return;
193  }
194  waypoint_set_lla(wp_id, lla);
195  if (waypoint_is_global(wp_id)) {
196  /* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */
198  DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id,
199  &lla->lat, &lla->lon, &hmsl);
200  } else {
201  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
202  &waypoints[wp_id].enu_i.x,
203  &waypoints[wp_id].enu_i.y,
204  &waypoints[wp_id].enu_i.z);
205  }
206 }
207 
209 void waypoint_set_latlon(uint8_t wp_id, struct LlaCoor_i *lla)
210 {
211  if (wp_id >= nb_waypoint) {
212  return;
213  }
214  waypoints[wp_id].lla.lat = lla->lat;
215  waypoints[wp_id].lla.lon = lla->lon;
216  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
217  waypoint_localize(wp_id);
218 }
219 
222 {
223  if (wp_id >= nb_waypoint) {
224  return;
225  }
226  if (waypoint_is_global(wp_id)) {
228  } else {
230  }
231 }
232 
235 {
236  if (wp_id >= nb_waypoint) {
237  return;
238  }
239  if (waypoint_is_global(wp_id)) {
241  } else {
243  }
244 }
245 
247 {
248  if (wp_id >= nb_waypoint) {
249  return;
250  }
251  if (waypoint_is_global(wp_id)) {
252  /* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */
253  struct LlaCoor_i *lla = &(waypoints[wp_id].lla);
255  DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id,
256  &lla->lat, &lla->lon, &hmsl);
257  } else {
258  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
259  &waypoints[wp_id].enu_i.x,
260  &waypoints[wp_id].enu_i.y,
261  &waypoints[wp_id].enu_i.z);
262  }
263 }
264 
266 {
267  if (state.ned_initialized_i) {
268  struct EcefCoor_i ecef;
270  lla_of_ecef_i(&waypoints[wp_id].lla, &ecef);
271  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
272  }
273 }
274 
277 {
278  if (state.ned_initialized_i) {
279  struct EnuCoor_i enu;
281  // convert ENU pos from cm to BFP with INT32_POS_FRAC
282  enu.x = POS_BFP_OF_REAL(enu.x) / 100;
283  enu.y = POS_BFP_OF_REAL(enu.y) / 100;
284  enu.z = POS_BFP_OF_REAL(enu.z) / 100;
285  waypoints[wp_id].enu_i = enu;
286  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
287  ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
288  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
289  }
290 }
291 
294 {
295  uint8_t i = 0;
296  for (i = 0; i < nb_waypoint; i++) {
297  if (waypoint_is_global(i)) {
299  }
300  }
301 }
302 
311 {
312  if (wp_id < nb_waypoint) {
313  if (!waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
314  waypoint_globalize(wp_id);
315  }
316  return &waypoints[wp_id].lla;
317  }
318  else {
319  return NULL;
320  }
321 }
322 
323 void waypoint_copy(uint8_t wp_dest, uint8_t wp_src)
324 {
325  if (wp_dest < nb_waypoint && wp_src < nb_waypoint) {
326  waypoints[wp_dest] = waypoints[wp_src];
327  }
328 }
329 
331 {
332  if (wp_dest < nb_waypoint && wp_src < nb_waypoint) {
333  waypoints[wp_dest].enu_f.x = waypoints[wp_src].enu_f.x;
334  waypoints[wp_dest].enu_f.y = waypoints[wp_src].enu_f.y;
335  waypoints[wp_dest].enu_i.x = waypoints[wp_src].enu_i.x;
336  waypoints[wp_dest].enu_i.y = waypoints[wp_src].enu_i.y;
337  waypoints[wp_dest].lla.lat = waypoints[wp_src].lla.lat;
338  waypoints[wp_dest].lla.lon = waypoints[wp_src].lla.lon;
339  }
340 }
void waypoint_set_global_flag(uint8_t wp_id)
Definition: waypoints.c:63
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.
int32_t y
North.
int32_t x
East.
struct EnuCoor_f enu_f
Definition: waypoints.h:41
vector in EarthCenteredEarthFixed coordinates
void waypoint_set_alt_i(uint8_t wp_id, int32_t alt)
Definition: waypoints.c:159
vector in East North Up coordinates Units: meters
#define POS_BFP_OF_REAL(_af)
void waypoint_move_here_2d(uint8_t wp_id)
Definition: waypoints.c:246
float waypoint_get_alt(uint8_t wp_id)
Get altitude of waypoint in meters (above reference)
Definition: waypoints.c:93
vector in Latitude, Longitude and Altitude
void waypoint_position_copy(uint8_t wp_dest, uint8_t wp_src)
Definition: waypoints.c:330
void waypoint_set_alt(uint8_t wp_id, float alt)
Set altitude of waypoint in meters (above reference)
Definition: waypoints.c:169
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
Definition: waypoints.c:189
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:138
#define WP_FLAG_GLOBAL
Definition: waypoints.h:33
#define WP_FLAG_ENU_F
Definition: waypoints.h:35
void waypoint_set_here(uint8_t wp_id)
set waypoint to current location and altitude
Definition: waypoints.c:221
struct Waypoint waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: waypoints.c:32
float x
in meters
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition: waypoints.c:77
void waypoint_set_latlon(uint8_t wp_id, struct LlaCoor_i *lla)
set waypoint latitude/longitude without updating altitude
Definition: waypoints.c:209
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition: waypoints.c:85
void waypoint_move_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos)
Definition: waypoints.c:125
#define ENU_FLOAT_OF_BFP(_o, _i)
struct LlaCoor_i lla
Reference point in lla.
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:323
int32_t lon
in degrees*1e7
struct LlaCoor_i lla
Definition: waypoints.h:42
int32_t z
Up.
const uint8_t nb_waypoint
Definition: waypoints.c:31
signed long int32_t
Definition: types.h:19
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
Definition: waypoints.c:101
#define WP_FLAG_LLA_I
Definition: waypoints.h:36
void waypoint_clear_global_flag(uint8_t wp_id)
Definition: waypoints.c:70
void waypoint_globalize(uint8_t wp_id)
update global LLA coordinates from its ENU coordinates
Definition: waypoints.c:265
vector in East North Up coordinates
#define WP_FLAG_ENU_I
Definition: waypoints.h:34
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
float z
in meters
void waypoint_set_here_2d(uint8_t wp_id)
set waypoint to current horizontal location without modifying altitude
Definition: waypoints.c:234
struct EnuCoor_i enu_i
with INT32_POS_FRAC
Definition: waypoints.h:40
bool waypoint_is_global(uint8_t wp_id)
Definition: waypoints.c:55
#define ENU_BFP_OF_REAL(_o, _i)
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition: waypoints.c:113
#define POS_FLOAT_OF_BFP(_ai)
void waypoint_move_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Definition: waypoints.c:150
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
uint8_t flags
bitmask encoding valid representations and if local or global
Definition: waypoints.h:39
static struct EnuCoor_i * stateGetPositionEnu_i(void)
Get position in local ENU coordinates (int).
Definition: state.h:674
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.
struct LlaCoor_i * waypoint_get_lla(uint8_t wp_id)
Get LLA coordinates of waypoint.
Definition: waypoints.c:310
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:293
int32_t lat
in degrees*1e7
float y
in meters
void waypoint_localize(uint8_t wp_id)
update local ENU coordinates from its LLA coordinates
Definition: waypoints.c:276
void waypoints_init(void)
initialize global and local waypoints
Definition: waypoints.c:35
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
struct State state
Definition: state.c:36
void waypoint_set_lla(uint8_t wp_id, struct LlaCoor_i *lla)
Definition: waypoints.c:179