Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
traffic_info.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Pascal Brisset, Antoine Drouin (2008), Kirk Scheper (2016)
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  */
28 
29 #include "generated/airframe.h" // AC_ID
30 #include "generated/flight_plan.h" // NAV_MSL0
31 
33 #include "pprzlink/dl_protocol.h" // datalink messages
34 #include "pprzlink/messages.h" // telemetry messages
35 
36 #include "state.h"
37 #include "math/pprz_geodetic_utm.h"
39 
40 /* number of ac being tracked */
42 /* index of ac in the list of traffic info aircraft (ti_acs) */
44 /* container for available traffic info */
46 
47 /* Geoid height (msl) over ellipsoid [mm] */
49 
51 {
52  memset(ti_acs_id, 0, NB_ACS_ID);
53 
54  ti_acs_id[0] = 0; // ground station
55  ti_acs_id[AC_ID] = 1;
56  ti_acs[ti_acs_id[AC_ID]].ac_id = AC_ID;
57  ti_acs_idx = 2;
58 
59  geoid_height = NAV_MSL0;
60 }
61 
66 static void update_geoid_height(void) {
68  {
70  } else if(bit_is_set(gps.valid_fields, GPS_VALID_POS_LLA_BIT))
71  {
73  } /* default is just keep last available height */
74 }
75 
76 bool parse_acinfo_dl(void)
77 {
78  uint8_t sender_id = SenderIdOfPprzMsg(dl_buffer);
79  uint8_t msg_id = IdOfPprzMsg(dl_buffer);
80 
81  /* handle telemetry message */
82 #if PPRZLINK_DEFAULT_VER == 2
83  if (pprzlink_get_msg_class_id(dl_buffer) == DL_telemetry_CLASS_ID) {
84 #else
85  if (sender_id > 0) {
86 #endif
87  switch (msg_id) {
88  case DL_GPS_SMALL: {
89  uint32_t multiplex_speed = DL_GPS_SMALL_multiplex_speed(dl_buffer);
90 
91  // decode compressed values
92  int16_t course = (int16_t)((multiplex_speed >> 21) & 0x7FF); // bits 31-21 course in decideg
93  if (course & 0x400) {
94  course |= 0xF800; // fix for twos complements
95  }
96  course *= 2; // scale course by resolution
97  int16_t gspeed = (int16_t)((multiplex_speed >> 10) & 0x7FF); // bits 20-10 ground speed cm/s
98  if (gspeed & 0x400) {
99  gspeed |= 0xF800; // fix for twos complements
100  }
101  int16_t climb = (int16_t)(multiplex_speed & 0x3FF); // bits 9-0 z climb speed in cm/s
102  if (climb & 0x200) {
103  climb |= 0xFC00; // fix for twos complements
104  }
105 
106  set_ac_info_lla(sender_id,
107  DL_GPS_SMALL_lat(dl_buffer),
108  DL_GPS_SMALL_lon(dl_buffer),
109  (int32_t)DL_GPS_SMALL_alt(dl_buffer) * 10,
110  course,
111  gspeed,
112  climb,
114  }
115  break;
116  case DL_GPS: {
117  set_ac_info_utm(sender_id,
118  DL_GPS_utm_east(dl_buffer),
119  DL_GPS_utm_north(dl_buffer),
120  DL_GPS_alt(dl_buffer),
121  DL_GPS_utm_zone(dl_buffer),
122  DL_GPS_course(dl_buffer),
123  DL_GPS_speed(dl_buffer),
124  DL_GPS_climb(dl_buffer),
125  DL_GPS_itow(dl_buffer));
126  }
127  break;
128  case DL_GPS_LLA: {
129  set_ac_info_lla(sender_id,
130  DL_GPS_LLA_lat(dl_buffer),
131  DL_GPS_LLA_lon(dl_buffer),
132  DL_GPS_LLA_alt(dl_buffer),
133  DL_GPS_LLA_course(dl_buffer),
134  DL_GPS_LLA_speed(dl_buffer),
135  DL_GPS_LLA_climb(dl_buffer),
136  DL_GPS_LLA_itow(dl_buffer));
137  }
138  break;
139  default:
140  return FALSE;
141  }
142  /* handle datalink message */
143  } else {
144  switch (msg_id) {
145  case DL_ACINFO: {
146  set_ac_info_utm(DL_ACINFO_ac_id(dl_buffer),
147  DL_ACINFO_utm_east(dl_buffer),
148  DL_ACINFO_utm_north(dl_buffer),
149  DL_ACINFO_alt(dl_buffer) * 10,
150  DL_ACINFO_utm_zone(dl_buffer),
151  DL_ACINFO_course(dl_buffer),
152  DL_ACINFO_speed(dl_buffer),
153  DL_ACINFO_climb(dl_buffer),
154  DL_ACINFO_itow(dl_buffer));
155  }
156  break;
157  case DL_ACINFO_LLA: {
158  set_ac_info_lla(DL_ACINFO_LLA_ac_id(dl_buffer),
159  DL_ACINFO_LLA_lat(dl_buffer),
160  DL_ACINFO_LLA_lon(dl_buffer),
161  DL_ACINFO_LLA_alt(dl_buffer) * 10,
162  DL_ACINFO_LLA_course(dl_buffer),
163  DL_ACINFO_LLA_speed(dl_buffer),
164  DL_ACINFO_LLA_climb(dl_buffer),
165  DL_ACINFO_LLA_itow(dl_buffer));
166  }
167  break;
168  default:
169  return FALSE;
170  }
171  }
172  return TRUE;
173 }
174 
175 
176 void set_ac_info_utm(uint8_t id, uint32_t utm_east, uint32_t utm_north, uint32_t alt, uint8_t utm_zone, uint16_t course,
178 {
179  if (ti_acs_idx < NB_ACS) {
180  if (id > 0 && ti_acs_id[id] == 0) { // new aircraft id
181  ti_acs_id[id] = ti_acs_idx++;
182  ti_acs[ti_acs_id[id]].ac_id = id;
183  }
184 
185  ti_acs[ti_acs_id[id]].status = 0;
186 
187  uint16_t my_zone = state.utm_origin_f.zone;
188  if (utm_zone == my_zone) {
189  ti_acs[ti_acs_id[id]].utm_pos_i.east = utm_east;
190  ti_acs[ti_acs_id[id]].utm_pos_i.north = utm_north;
191  ti_acs[ti_acs_id[id]].utm_pos_i.alt = alt;
192  ti_acs[ti_acs_id[id]].utm_pos_i.zone = utm_zone;
194 
195  } else { // store other uav in utm extended zone
196  struct UtmCoor_i utm = {.east = utm_east, .north = utm_north, .alt = alt, .zone = utm_zone};
197  struct LlaCoor_i lla;
198  lla_of_utm_i(&lla, &utm);
199  LLA_COPY(ti_acs[ti_acs_id[id]].lla_pos_i, lla);
201 
202  utm.zone = my_zone;
203  utm_of_lla_i(&utm, &lla);
204 
205  UTM_COPY(ti_acs[ti_acs_id[id]].utm_pos_i, utm);
206  SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_POS_UTM_I);
207  }
208 
209  ti_acs[ti_acs_id[id]].course = RadOfDeciDeg(course);
210  ti_acs[ti_acs_id[id]].gspeed = MOfCm(gspeed);
211  ti_acs[ti_acs_id[id]].climb = MOfCm(climb);
213 
214  ti_acs[ti_acs_id[id]].itow = itow;
215  }
216 }
217 
219  int16_t course, uint16_t gspeed, int16_t climb, uint32_t itow)
220 {
221  if (ti_acs_idx < NB_ACS) {
222  if (id > 0 && ti_acs_id[id] == 0) {
223  ti_acs_id[id] = ti_acs_idx++;
224  ti_acs[ti_acs_id[id]].ac_id = id;
225  }
226 
227  ti_acs[ti_acs_id[id]].status = 0;
228 
229  struct LlaCoor_i lla = {.lat = lat, .lon = lon, .alt = alt};
230  LLA_COPY(ti_acs[ti_acs_id[id]].lla_pos_i, lla);
232 
233  ti_acs[ti_acs_id[id]].course = RadOfDeciDeg(course);
234  ti_acs[ti_acs_id[id]].gspeed = MOfCm(gspeed);
235  ti_acs[ti_acs_id[id]].climb = MOfCm(climb);
236  SetBit(ti_acs[ti_acs_id[id]].status, AC_INFO_VEL_LOCAL_F);
237 
238  ti_acs[ti_acs_id[id]].itow = itow;
239  }
240 }
241 
242 /* Conversion funcitons for computing ac position in requested reference frame from
243  * any other available reference frame
244  */
245 
246 /* compute UTM position of aircraft with ac_id (int) */
248 {
249  uint8_t ac_nr = ti_acs_id[ac_id];
250  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
251  {
252  return;
253  }
254 
255  /* LLA_i -> UTM_i is more accurate than from UTM_f */
256  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
257  {
258  // use my zone as reference, i.e zone extend
260  utm_of_lla_i(&ti_acs[ac_nr].utm_pos_i, &ti_acs[ac_nr].lla_pos_i);
262  ti_acs[ac_nr].utm_pos_i.alt -= geoid_height;
263  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
264  {
265  UTM_BFP_OF_REAL(ti_acs[ac_nr].utm_pos_i, ti_acs[ac_nr].utm_pos_f);
266  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
267  {
268  // use my zone as reference, i.e zone extend
270  utm_of_lla_f(&ti_acs[ac_nr].utm_pos_f, &ti_acs[ac_nr].lla_pos_f);
272  ti_acs[ac_nr].utm_pos_f.alt -= geoid_height/1000.;
273  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F);
274  UTM_BFP_OF_REAL(ti_acs[ac_nr].utm_pos_i, ti_acs[ac_nr].utm_pos_f);
275  }
276  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I);
277 }
278 
279 /* compute LLA position of aircraft with ac_id (int) */
281 {
282  uint8_t ac_nr = ti_acs_id[ac_id];
283  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
284  {
285  return;
286  }
287 
288  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
289  {
290  LLA_BFP_OF_REAL(ti_acs[ac_nr].lla_pos_i, ti_acs[ac_nr].lla_pos_f);
291  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
292  {
293  lla_of_utm_i(&ti_acs[ac_nr].lla_pos_i, &ti_acs[ac_nr].utm_pos_i);
295  ti_acs[ac_nr].lla_pos_i.alt += geoid_height;
296  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
297  {
298  lla_of_utm_f(&ti_acs[ac_nr].lla_pos_f, &ti_acs[ac_nr].utm_pos_f);
300  ti_acs[ac_nr].lla_pos_f.alt += geoid_height/1000.;
301  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F);
302  LLA_BFP_OF_REAL(ti_acs[ac_nr].lla_pos_i, ti_acs[ac_nr].lla_pos_f);
303  }
304  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I);
305 }
306 
307 /* compute ENU position of aircraft with ac_id (int) */
309 {
310  uint8_t ac_nr = ti_acs_id[ac_id];
311  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I))
312  {
313  return;
314  }
315 
316  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F))
317  {
318  ENU_BFP_OF_REAL(ti_acs[ac_nr].enu_pos_i, ti_acs[ac_nr].enu_pos_f);
319  }
320  else if (state.ned_initialized_i)
321  {
322  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
323  {
325  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
326  {
328  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F);
329  ENU_BFP_OF_REAL(ti_acs[ac_nr].enu_pos_i, ti_acs[ac_nr].enu_pos_f)
330  }
331  } else if (state.utm_initialized_f)
332  {
333  /* if utm origin is initialized we use the ENU = UTM - UTM_ORIGIN as in state to facilitate comparison */
334  ENU_OF_UTM_DIFF(ti_acs[ac_nr].enu_pos_f, *acInfoGetPositionUtm_f(ac_id), state.utm_origin_f);
335  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F);
336  ENU_BFP_OF_REAL(ti_acs[ac_nr].enu_pos_i, ti_acs[ac_nr].enu_pos_f);
337  }
338  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I);
339 }
340 
341 /* compute UTM position of aircraft with ac_id (float) */
343 {
344  uint8_t ac_nr = ti_acs_id[ac_id];
345  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
346  {
347  return;
348  }
349 
350  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
351  {
352  UTM_FLOAT_OF_BFP(ti_acs[ac_nr].utm_pos_f, ti_acs[ac_nr].utm_pos_i);
353  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
354  {
355  // use my zone as reference, i.e zone extend
357  utm_of_lla_i(&ti_acs[ac_nr].utm_pos_i, &ti_acs[ac_nr].lla_pos_i);
359  ti_acs[ac_nr].utm_pos_i.alt -= geoid_height;
360  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I);
361  UTM_FLOAT_OF_BFP(ti_acs[ac_nr].utm_pos_f, ti_acs[ac_nr].utm_pos_i);
362  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
363  {
364  /* not very accurate with float ~5cm */
366  utm_of_lla_f(&ti_acs[ac_nr].utm_pos_f, &ti_acs[ac_nr].lla_pos_f);
368  ti_acs[ac_nr].utm_pos_f.alt -= geoid_height/1000.;
369  }
370  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F);
371 }
372 
373 /* compute LLA position of aircraft with ac_id (float) */
375 {
376  uint8_t ac_nr = ti_acs_id[ac_id];
377  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
378  {
379  return;
380  }
381 
382  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
383  {
384  LLA_FLOAT_OF_BFP(ti_acs[ac_nr].lla_pos_f, ti_acs[ac_nr].lla_pos_i);
385  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
386  {
387  lla_of_utm_i(&ti_acs[ac_nr].lla_pos_i, &ti_acs[ac_nr].utm_pos_i);
389  ti_acs[ac_nr].lla_pos_i.alt += geoid_height;
390  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I);
391  LLA_FLOAT_OF_BFP(ti_acs[ac_nr].lla_pos_f, ti_acs[ac_nr].lla_pos_i);
392  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
393  {
394  lla_of_utm_f(&ti_acs[ac_nr].lla_pos_f, &ti_acs[ac_nr].utm_pos_f);
396  ti_acs[ac_nr].lla_pos_f.alt += geoid_height/1000.;
397  }
398  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F);
399 }
400 
401 /* compute ENU position of aircraft with ac_id (float) */
403 {
404  uint8_t ac_nr = ti_acs_id[ac_id];
405  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F))
406  {
407  return;
408  }
409 
410  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I))
411  {
412  ENU_FLOAT_OF_BFP(ti_acs[ac_nr].enu_pos_f, ti_acs[ac_nr].enu_pos_i);
413  }
414  else if (state.ned_initialized_i)
415  {
416  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
417  {
419  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I) || bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
420  {
422  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I);
423  ENU_FLOAT_OF_BFP(ti_acs[ac_nr].enu_pos_f, ti_acs[ac_nr].enu_pos_i);
424  }
425  } else if (state.utm_initialized_f)
426  {
427  /* if utm origin is initialized we use the ENU = UTM - UTM_ORIGIN as in state to facilitate comparison */
428  ENU_OF_UTM_DIFF(ti_acs[ac_nr].enu_pos_f, *acInfoGetPositionUtm_f(ac_id), state.utm_origin_f);
429  }
430  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F);
431 }
432 
433 /* compute ENU velocity of aircraft with ac_id (int) */
435 {
436  uint8_t ac_nr = ti_acs_id[ac_id];
437  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I))
438  {
439  return;
440  }
441 
442  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F))
443  {
444  SPEEDS_BFP_OF_REAL(ti_acs[ac_nr].enu_vel_i, ti_acs[ac_nr].enu_vel_f);
445  } else {
446  ti_acs[ac_nr].enu_vel_f.x = ti_acs[ac_nr].gspeed * sinf(ti_acs[ac_nr].course);
447  ti_acs[ac_nr].enu_vel_f.y = ti_acs[ac_nr].gspeed * cosf(ti_acs[ac_nr].course);
448  ti_acs[ac_nr].enu_vel_f.z = ti_acs[ac_nr].climb;
449  SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F);
450  SPEEDS_BFP_OF_REAL(ti_acs[ac_nr].enu_vel_i, ti_acs[ac_nr].enu_vel_f);
451  }
452  SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I);
453 }
454 
455 /* compute ENU position of aircraft with ac_id (float) */
457 {
458  uint8_t ac_nr = ti_acs_id[ac_id];
459  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F))
460  {
461  return;
462  }
463 
464  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I))
465  {
466  SPEEDS_FLOAT_OF_BFP(ti_acs[ac_nr].enu_vel_f, ti_acs[ac_nr].enu_vel_i);
467  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_LOCAL_F)) {
468  ti_acs[ac_nr].enu_vel_f.x = ti_acs[ac_nr].gspeed * sinf(ti_acs[ac_nr].course);
469  ti_acs[ac_nr].enu_vel_f.y = ti_acs[ac_nr].gspeed * cosf(ti_acs[ac_nr].course);
470  ti_acs[ac_nr].enu_vel_f.z = ti_acs[ac_nr].climb;
471  }
472  SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F);
473 }
static struct UtmCoor_f * acInfoGetPositionUtm_f(uint8_t ac_id)
Get position from UTM coordinates (float).
Definition: traffic_info.h:361
unsigned short uint16_t
Definition: types.h:16
uint8_t ac_id
Definition: sim_ap.c:45
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:733
status
Definition: anemotaxis.c:10
int32_t north
in centimeters
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
void acInfoCalcPositionUtm_f(uint8_t ac_id)
Definition: traffic_info.c:342
struct LlaCoor_i lla_pos_i
Position in Latitude, Longitude and Altitude.
Definition: traffic_info.h:78
int32_t geoid_height
Definition: traffic_info.c:48
struct EnuCoor_f enu_vel_f
speed in North East Down coordinates
Definition: traffic_info.h:114
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
void set_ac_info_lla(uint8_t id, int32_t lat, int32_t lon, int32_t alt, int16_t course, uint16_t gspeed, int16_t climb, uint32_t itow)
Set Aircraft info.
Definition: traffic_info.c:218
void acInfoCalcVelocityEnu_f(uint8_t ac_id)
Definition: traffic_info.c:456
void acInfoCalcVelocityEnu_i(uint8_t ac_id)
Definition: traffic_info.c:434
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:82
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
float climb
m/s
Definition: traffic_info.h:118
uint32_t itow
ms
Definition: traffic_info.h:119
void acInfoCalcPositionUtm_i(uint8_t ac_id)
Definition: traffic_info.c:247
void acInfoCalcPositionLla_i(uint8_t ac_id)
Definition: traffic_info.c:280
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition: traffic_info.h:97
#define AC_INFO_POS_LLA_I
Definition: traffic_info.h:48
float gspeed
m/s
Definition: traffic_info.h:117
#define AC_INFO_POS_UTM_F
Definition: traffic_info.h:50
bool parse_acinfo_dl(void)
Parse all datalink or telemetry messages that contain global position of other acs Messages currently...
Definition: traffic_info.c:76
#define UTM_FLOAT_OF_BFP(_o, _i)
#define AC_INFO_VEL_ENU_I
Definition: traffic_info.h:53
int32_t east
in centimeters
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:727
vector in Latitude, Longitude and Altitude
volatile uint32_t nb_tick
SYS_TIME_TICKS since startup.
Definition: sys_time.h:74
uint16_t status
Holds the status bits for all acinfo position and velocity representations.
Definition: traffic_info.h:64
#define NB_ACS_ID
Definition: rssi.c:35
uint8_t ac_id
Definition: traffic_info.h:58
#define FALSE
Definition: std.h:5
int32_t alt
in millimeters above WGS84 reference ellipsoid
WGS-84 Geoid Heights.
void set_ac_info_utm(uint8_t id, uint32_t utm_east, uint32_t utm_north, uint32_t alt, uint8_t utm_zone, uint16_t course, uint16_t gspeed, uint16_t climb, uint32_t itow)
Set Aircraft info.
Definition: traffic_info.c:176
uint8_t zone
UTM zone number.
#define UTM_BFP_OF_REAL(_o, _i)
#define TRUE
Definition: std.h:4
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:88
void acInfoCalcPositionEnu_i(uint8_t ac_id)
Definition: traffic_info.c:308
float x
in meters
struct UtmCoor_i utm_pos_i
Position in UTM coordinates.
Definition: traffic_info.h:71
#define NB_ACS
Definition: rssi.c:38
static void update_geoid_height(void)
Update estimate of the geoid height Requires an available hsml and/or lla measurement, if not available value isn't updated.
Definition: traffic_info.c:66
void lla_of_utm_i(struct LlaCoor_i *lla, struct UtmCoor_i *utm)
Convert a UTM to LLA.
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
#define ENU_FLOAT_OF_BFP(_o, _i)
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
int32_t lon
in degrees*1e7
#define AC_INFO_POS_ENU_I
Definition: traffic_info.h:49
static struct LlaCoor_f * acInfoGetPositionLla_f(uint8_t ac_id)
Get position from LLA coordinates (float).
Definition: traffic_info.h:372
void utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla)
Convert a LLA to UTM.
uint8_t zone
UTM zone number.
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
#define LLA_COPY(_pos1, _pos2)
Definition: pprz_geodetic.h:53
signed long int32_t
Definition: types.h:19
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
#define AC_INFO_POS_ENU_F
Definition: traffic_info.h:52
void traffic_info_init(void)
Definition: traffic_info.c:50
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
float alt
in meters (normally above WGS84 reference ellipsoid)
Constants UTM (Mercator) projections.
void acInfoCalcPositionEnu_f(uint8_t ac_id)
Definition: traffic_info.c:402
int32_t alt
in millimeters (above WGS84 reference ellipsoid or above MSL)
struct LlaCoor_f lla_pos_f
Position in Latitude, Longitude and Altitude.
Definition: traffic_info.h:104
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
float z
in meters
#define AC_INFO_VEL_LOCAL_F
Definition: traffic_info.h:55
static int16_t course[3]
Definition: airspeed_uADC.c:57
struct acInfo ti_acs[NB_ACS]
Definition: traffic_info.c:45
#define LLA_BFP_OF_REAL(_o, _i)
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:49
uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)
Convert time in sys_time ticks to GPS time of week.
Definition: gps.c:360
#define ENU_BFP_OF_REAL(_o, _i)
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:86
#define AC_INFO_POS_UTM_I
Definition: traffic_info.h:47
float course
rad
Definition: traffic_info.h:116
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.
int32_t lat
in degrees*1e7
void acInfoCalcPositionLla_f(uint8_t ac_id)
Definition: traffic_info.c:374
static struct LlaCoor_i * acInfoGetPositionLla_i(uint8_t ac_id)
Get position from LLA coordinates (int).
Definition: traffic_info.h:339
uint8_t ti_acs_idx
Definition: traffic_info.c:41
float y
in meters
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:73
struct GpsState gps
global GPS state
Definition: gps.c:75
#define AC_INFO_POS_LLA_F
Definition: traffic_info.h:51
#define LLA_FLOAT_OF_BFP(_o, _i)
static int32_t wgs84_ellipsoid_to_geoid_i(int32_t lat, int32_t lon)
Get WGS84 ellipsoid/geoid separation.
struct State state
Definition: state.c:36
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
#define AC_INFO_VEL_ENU_F
Definition: traffic_info.h:54
position in UTM coordinates
uint8_t ti_acs_id[NB_ACS_ID]
Definition: traffic_info.c:43
#define UTM_COPY(_u1, _u2)
Definition: pprz_geodetic.h:66