Paparazzi UAS  v5.15_devel-50-g4d7045d
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  waypoints[wp_id].enu_i.z = alt;
154  /* also update ENU float representation */
155  waypoints[wp_id].enu_f.z = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.z);
156  waypoint_globalize(wp_id);
157  }
158 }
159 
160 void waypoint_set_alt(uint8_t wp_id, float alt)
161 {
162  if (wp_id < nb_waypoint) {
163  waypoints[wp_id].enu_f.z = alt;
164  /* also update ENU fixed point representation */
165  waypoints[wp_id].enu_i.z = POS_BFP_OF_REAL(waypoints[wp_id].enu_f.z);
166  waypoint_globalize(wp_id);
167  }
168 }
169 
170 void waypoint_set_lla(uint8_t wp_id, struct LlaCoor_i *lla)
171 {
172  if (wp_id >= nb_waypoint) {
173  return;
174  }
175  waypoints[wp_id].lla = *lla;
176  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
177  waypoint_localize(wp_id);
178 }
179 
180 void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla)
181 {
182  if (wp_id >= nb_waypoint) {
183  return;
184  }
185  waypoint_set_lla(wp_id, lla);
186  if (waypoint_is_global(wp_id)) {
187  /* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */
189  DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id,
190  &lla->lat, &lla->lon, &hmsl);
191  } else {
192  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
193  &waypoints[wp_id].enu_i.x,
194  &waypoints[wp_id].enu_i.y,
195  &waypoints[wp_id].enu_i.z);
196  }
197 }
198 
200 void waypoint_set_latlon(uint8_t wp_id, struct LlaCoor_i *lla)
201 {
202  if (wp_id >= nb_waypoint) {
203  return;
204  }
205  waypoints[wp_id].lla.lat = lla->lat;
206  waypoints[wp_id].lla.lon = lla->lon;
207  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
208  waypoint_localize(wp_id);
209 }
210 
213 {
214  if (wp_id >= nb_waypoint) {
215  return;
216  }
217  if (waypoint_is_global(wp_id)) {
219  } else {
221  }
222 }
223 
226 {
227  if (wp_id >= nb_waypoint) {
228  return;
229  }
230  if (waypoint_is_global(wp_id)) {
232  } else {
234  }
235 }
236 
238 {
239  if (state.ned_initialized_i) {
240  struct EcefCoor_i ecef;
242  lla_of_ecef_i(&waypoints[wp_id].lla, &ecef);
243  SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I);
244  }
245 }
246 
249 {
250  if (state.ned_initialized_i) {
251  struct EnuCoor_i enu;
253  // convert ENU pos from cm to BFP with INT32_POS_FRAC
254  enu.x = POS_BFP_OF_REAL(enu.x) / 100;
255  enu.y = POS_BFP_OF_REAL(enu.y) / 100;
256  enu.z = POS_BFP_OF_REAL(enu.z) / 100;
257  waypoints[wp_id].enu_i = enu;
258  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
259  ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);
260  SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F);
261  }
262 }
263 
266 {
267  uint8_t i = 0;
268  for (i = 0; i < nb_waypoint; i++) {
269  if (waypoint_is_global(i)) {
271  }
272  }
273 }
274 
283 {
284  if (wp_id < nb_waypoint) {
285  if (!waypoint_is_global(wp_id) && !bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) {
286  waypoint_globalize(wp_id);
287  }
288  return &waypoints[wp_id].lla;
289  }
290  else {
291  return NULL;
292  }
293 }
294 
295 void waypoint_copy(uint8_t wp_dest, uint8_t wp_src)
296 {
297  if (wp_dest < nb_waypoint && wp_src < nb_waypoint) {
298  waypoints[wp_dest] = waypoints[wp_src];
299  }
300 }
301 
303 {
304  if (wp_dest < nb_waypoint && wp_src < nb_waypoint) {
305  waypoints[wp_dest].enu_f.x = waypoints[wp_src].enu_f.x;
306  waypoints[wp_dest].enu_f.y = waypoints[wp_src].enu_f.y;
307  waypoints[wp_dest].enu_i.x = waypoints[wp_src].enu_i.x;
308  waypoints[wp_dest].enu_i.y = waypoints[wp_src].enu_i.y;
309  waypoints[wp_dest].lla.lat = waypoints[wp_src].lla.lat;
310  waypoints[wp_dest].lla.lon = waypoints[wp_src].lla.lon;
311  }
312 }
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:150
vector in East North Up coordinates Units: meters
#define POS_BFP_OF_REAL(_af)
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:302
void waypoint_set_alt(uint8_t wp_id, float alt)
Set altitude of waypoint in meters (above reference)
Definition: waypoints.c:160
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:180
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:212
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:200
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:295
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:237
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:225
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 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:282
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition: waypoints.c:265
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:248
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:170