Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 */
45 struct acInfo ti_acs[NB_ACS];
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 
77 {
78  uint8_t sender_id = SenderIdOfPprzMsg(buf);
79  uint8_t msg_id = IdOfPprzMsg(buf);
80 
81  /* handle telemetry message */
82 #if PPRZLINK_DEFAULT_VER == 2
83  if (pprzlink_get_msg_class_id(buf) == 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(buf);
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(buf),
108  DL_GPS_SMALL_lon(buf),
109  (int32_t)DL_GPS_SMALL_alt(buf) * 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(buf),
119  DL_GPS_utm_north(buf),
120  DL_GPS_alt(buf),
121  DL_GPS_utm_zone(buf),
122  DL_GPS_course(buf),
123  DL_GPS_speed(buf),
124  DL_GPS_climb(buf),
125  DL_GPS_itow(buf));
126  }
127  break;
128  case DL_GPS_LLA: {
129  set_ac_info_lla(sender_id,
130  DL_GPS_LLA_lat(buf),
131  DL_GPS_LLA_lon(buf),
132  DL_GPS_LLA_alt(buf),
133  DL_GPS_LLA_course(buf),
134  DL_GPS_LLA_speed(buf),
135  DL_GPS_LLA_climb(buf),
136  DL_GPS_LLA_itow(buf));
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(buf),
147  DL_ACINFO_utm_east(buf),
148  DL_ACINFO_utm_north(buf),
149  DL_ACINFO_alt(buf) * 10,
150  DL_ACINFO_utm_zone(buf),
151  DL_ACINFO_course(buf),
152  DL_ACINFO_speed(buf),
153  DL_ACINFO_climb(buf),
154  DL_ACINFO_itow(buf));
155  }
156  break;
157  case DL_ACINFO_LLA: {
158  set_ac_info_lla(DL_ACINFO_LLA_ac_id(buf),
159  DL_ACINFO_LLA_lat(buf),
160  DL_ACINFO_LLA_lon(buf),
161  DL_ACINFO_LLA_alt(buf) * 10,
162  DL_ACINFO_LLA_course(buf),
163  DL_ACINFO_LLA_speed(buf),
164  DL_ACINFO_LLA_climb(buf),
165  DL_ACINFO_LLA_itow(buf));
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);
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);
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  {
324  struct EnuCoor_i enu;
326  // convert ENU pos from cm to BFP with INT32_POS_FRAC
327  enu.x = POS_BFP_OF_REAL(enu.x) / 100;
328  enu.y = POS_BFP_OF_REAL(enu.y) / 100;
329  enu.z = POS_BFP_OF_REAL(enu.z) / 100;
330  ti_acs[ac_nr].enu_pos_i = enu;
331  } 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))
332  {
334  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F);
335  ENU_BFP_OF_REAL(ti_acs[ac_nr].enu_pos_i, ti_acs[ac_nr].enu_pos_f)
336  }
337  } else if (state.utm_initialized_f)
338  {
339  /* if utm origin is initialized we use the ENU = UTM - UTM_ORIGIN as in state to facilitate comparison */
340  ENU_OF_UTM_DIFF(ti_acs[ac_nr].enu_pos_f, *acInfoGetPositionUtm_f(ac_id), state.utm_origin_f);
341  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F);
342  ENU_BFP_OF_REAL(ti_acs[ac_nr].enu_pos_i, ti_acs[ac_nr].enu_pos_f);
343  }
344  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I);
345 }
346 
347 /* compute UTM position of aircraft with ac_id (float) */
349 {
350  uint8_t ac_nr = ti_acs_id[ac_id];
351  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
352  {
353  return;
354  }
355 
356  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
357  {
358  UTM_FLOAT_OF_BFP(ti_acs[ac_nr].utm_pos_f, ti_acs[ac_nr].utm_pos_i);
359  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
360  {
361  // use my zone as reference, i.e zone extend
363  utm_of_lla_i(&ti_acs[ac_nr].utm_pos_i, &ti_acs[ac_nr].lla_pos_i);
365  ti_acs[ac_nr].utm_pos_i.alt -= geoid_height;
366  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I);
367  UTM_FLOAT_OF_BFP(ti_acs[ac_nr].utm_pos_f, ti_acs[ac_nr].utm_pos_i);
368  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
369  {
370  /* not very accurate with float ~5cm */
372  utm_of_lla_f(&ti_acs[ac_nr].utm_pos_f, &ti_acs[ac_nr].lla_pos_f);
374  ti_acs[ac_nr].utm_pos_f.alt -= geoid_height/1000.;
375  }
376  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F);
377 }
378 
379 /* compute LLA position of aircraft with ac_id (float) */
381 {
382  uint8_t ac_nr = ti_acs_id[ac_id];
383  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F))
384  {
385  return;
386  }
387 
388  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I))
389  {
390  LLA_FLOAT_OF_BFP(ti_acs[ac_nr].lla_pos_f, ti_acs[ac_nr].lla_pos_i);
391  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_I))
392  {
393  lla_of_utm_i(&ti_acs[ac_nr].lla_pos_i, &ti_acs[ac_nr].utm_pos_i);
395  ti_acs[ac_nr].lla_pos_i.alt += geoid_height;
396  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_I);
397  LLA_FLOAT_OF_BFP(ti_acs[ac_nr].lla_pos_f, ti_acs[ac_nr].lla_pos_i);
398  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_UTM_F))
399  {
400  lla_of_utm_f(&ti_acs[ac_nr].lla_pos_f, &ti_acs[ac_nr].utm_pos_f);
402  ti_acs[ac_nr].lla_pos_f.alt += geoid_height/1000.;
403  }
404  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_LLA_F);
405 }
406 
407 /* compute ENU position of aircraft with ac_id (float) */
409 {
410  uint8_t ac_nr = ti_acs_id[ac_id];
411  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F))
412  {
413  return;
414  }
415 
416  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I))
417  {
418  ENU_FLOAT_OF_BFP(ti_acs[ac_nr].enu_pos_f, ti_acs[ac_nr].enu_pos_i);
419  }
420  else if (state.ned_initialized_i)
421  {
422  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))
423  {
425  } 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))
426  {
427  struct EnuCoor_i enu;
429  // convert ENU pos from cm to BFP with INT32_POS_FRAC
430  enu.x = POS_BFP_OF_REAL(enu.x) / 100;
431  enu.y = POS_BFP_OF_REAL(enu.y) / 100;
432  enu.z = POS_BFP_OF_REAL(enu.z) / 100;
433  ti_acs[ac_nr].enu_pos_i = enu;
434  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_I);
435  ENU_FLOAT_OF_BFP(ti_acs[ac_nr].enu_pos_f, ti_acs[ac_nr].enu_pos_i);
436  }
437  } else if (state.utm_initialized_f)
438  {
439  /* if utm origin is initialized we use the ENU = UTM - UTM_ORIGIN as in state to facilitate comparison */
440  ENU_OF_UTM_DIFF(ti_acs[ac_nr].enu_pos_f, *acInfoGetPositionUtm_f(ac_id), state.utm_origin_f);
441  }
442  SetBit(ti_acs[ac_nr].status, AC_INFO_POS_ENU_F);
443 }
444 
445 /* compute ENU velocity of aircraft with ac_id (int) */
447 {
448  uint8_t ac_nr = ti_acs_id[ac_id];
449  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I))
450  {
451  return;
452  }
453 
454  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F))
455  {
456  SPEEDS_BFP_OF_REAL(ti_acs[ac_nr].enu_vel_i, ti_acs[ac_nr].enu_vel_f);
457  } else {
458  ti_acs[ac_nr].enu_vel_f.x = ti_acs[ac_nr].gspeed * sinf(ti_acs[ac_nr].course);
459  ti_acs[ac_nr].enu_vel_f.y = ti_acs[ac_nr].gspeed * cosf(ti_acs[ac_nr].course);
460  ti_acs[ac_nr].enu_vel_f.z = ti_acs[ac_nr].climb;
461  SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F);
462  SPEEDS_BFP_OF_REAL(ti_acs[ac_nr].enu_vel_i, ti_acs[ac_nr].enu_vel_f);
463  }
464  SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I);
465 }
466 
467 /* compute ENU position of aircraft with ac_id (float) */
469 {
470  uint8_t ac_nr = ti_acs_id[ac_id];
471  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F))
472  {
473  return;
474  }
475 
476  if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_I))
477  {
478  SPEEDS_FLOAT_OF_BFP(ti_acs[ac_nr].enu_vel_f, ti_acs[ac_nr].enu_vel_i);
479  } else if (bit_is_set(ti_acs[ac_nr].status, AC_INFO_VEL_LOCAL_F)) {
480  ti_acs[ac_nr].enu_vel_f.x = ti_acs[ac_nr].gspeed * sinf(ti_acs[ac_nr].course);
481  ti_acs[ac_nr].enu_vel_f.y = ti_acs[ac_nr].gspeed * cosf(ti_acs[ac_nr].course);
482  ti_acs[ac_nr].enu_vel_f.z = ti_acs[ac_nr].climb;
483  }
484  SetBit(ti_acs[ac_nr].status, AC_INFO_VEL_ENU_F);
485 }
static int16_t course[3]
Definition: airspeed_uADC.c:58
struct GpsState gps
global GPS state
Definition: gps.c:69
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:365
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:93
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:91
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:48
#define GPS_VALID_HMSL_BIT
Definition: gps.h:52
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:87
struct LlaCoor_f lla_pos_f
Position in Latitude, Longitude and Altitude.
Definition: traffic_info.h:104
struct EnuCoor_f enu_vel_f
speed in North East Down coordinates
Definition: traffic_info.h:114
uint8_t ac_id
Definition: traffic_info.h:58
uint32_t itow
ms
Definition: traffic_info.h:119
float climb
m/s
Definition: traffic_info.h:118
struct LlaCoor_i lla_pos_i
Position in Latitude, Longitude and Altitude.
Definition: traffic_info.h:78
float gspeed
m/s
Definition: traffic_info.h:117
struct UtmCoor_i utm_pos_i
Position in UTM coordinates.
Definition: traffic_info.h:71
uint16_t status
Holds the status bits for all acinfo position and velocity representations.
Definition: traffic_info.h:64
struct UtmCoor_f utm_pos_f
Position in UTM coordinates.
Definition: traffic_info.h:97
float course
rad
Definition: traffic_info.h:116
struct EnuCoor_i enu_pos_i
Position in North East Down coordinates.
Definition: traffic_info.h:84
#define AC_INFO_VEL_ENU_I
Definition: traffic_info.h:53
bool parse_acinfo_dl(uint8_t *buf)
Parse all datalink or telemetry messages that contain global position of other acs Messages currently...
Definition: traffic_info.c:76
#define AC_INFO_POS_LLA_I
Definition: traffic_info.h:48
uint8_t ti_acs_idx
Definition: traffic_info.c:41
#define AC_INFO_VEL_ENU_F
Definition: traffic_info.h:54
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
void acInfoCalcPositionUtm_f(uint8_t ac_id)
Definition: traffic_info.c:348
#define AC_INFO_POS_LLA_F
Definition: traffic_info.h:51
void acInfoCalcPositionEnu_f(uint8_t ac_id)
Definition: traffic_info.c:408
void acInfoCalcVelocityEnu_f(uint8_t ac_id)
Definition: traffic_info.c:468
void acInfoCalcPositionUtm_i(uint8_t ac_id)
Definition: traffic_info.c:247
void acInfoCalcPositionLla_i(uint8_t ac_id)
Definition: traffic_info.c:280
void acInfoCalcPositionLla_f(uint8_t ac_id)
Definition: traffic_info.c:380
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
static struct UtmCoor_f * acInfoGetPositionUtm_f(uint8_t ac_id)
Get position from UTM coordinates (float).
Definition: traffic_info.h:361
#define AC_INFO_POS_ENU_F
Definition: traffic_info.h:52
static struct LlaCoor_f * acInfoGetPositionLla_f(uint8_t ac_id)
Get position from LLA coordinates (float).
Definition: traffic_info.h:372
#define AC_INFO_POS_UTM_F
Definition: traffic_info.h:50
#define AC_INFO_POS_UTM_I
Definition: traffic_info.h:47
#define AC_INFO_POS_ENU_I
Definition: traffic_info.h:49
void traffic_info_init(void)
Definition: traffic_info.c:50
#define AC_INFO_VEL_LOCAL_F
Definition: traffic_info.h:55
struct acInfo ti_acs[NB_ACS]
Definition: traffic_info.c:45
uint8_t ti_acs_id[NB_ACS_ID]
Definition: traffic_info.c:43
void acInfoCalcPositionEnu_i(uint8_t ac_id)
Definition: traffic_info.c:308
void acInfoCalcVelocityEnu_i(uint8_t ac_id)
Definition: traffic_info.c:446
static struct LlaCoor_i * acInfoGetPositionLla_i(uint8_t ac_id)
Get position from LLA coordinates (int).
Definition: traffic_info.h:339
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:789
#define SPEEDS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:783
#define POS_BFP_OF_REAL(_af)
#define LLA_COPY(_pos1, _pos2)
Definition: pprz_geodetic.h:58
#define ENU_OF_UTM_DIFF(_pos, _utm1, _utm2)
Definition: pprz_geodetic.h:78
#define UTM_COPY(_u1, _u2)
Definition: pprz_geodetic.h:71
int32_t lat
in degrees*1e7
int32_t y
North.
int32_t alt
in millimeters (above WGS84 reference ellipsoid or above MSL)
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Up.
uint8_t zone
UTM zone number.
int32_t east
in centimeters
int32_t x
East.
int32_t lon
in degrees*1e7
int32_t north
in centimeters
#define ENU_FLOAT_OF_BFP(_o, _i)
void lla_of_utm_i(struct LlaCoor_i *lla, struct UtmCoor_i *utm)
Convert a UTM to LLA.
#define ENU_BFP_OF_REAL(_o, _i)
#define LLA_BFP_OF_REAL(_o, _i)
#define UTM_BFP_OF_REAL(_o, _i)
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.
#define LLA_FLOAT_OF_BFP(_o, _i)
void utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla)
Convert a LLA to UTM.
#define UTM_FLOAT_OF_BFP(_o, _i)
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
position in UTM coordinates
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
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
uint8_t status
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla)
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
float alt
in meters (normally above WGS84 reference ellipsoid)
float y
in meters
float x
in meters
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone
UTM zone number.
float z
in meters
Constants UTM (Mercator) projections.
WGS-84 Geoid Heights.
#define NB_ACS_ID
Definition: rssi.c:35
#define NB_ACS
Definition: rssi.c:38
API to get/set the generic vehicle states.
#define TRUE
Definition: std.h:4
#define FALSE
Definition: std.h:5
volatile uint32_t nb_tick
SYS_TIME_TICKS since startup.
Definition: sys_time.h:74
static void update_geoid_height(void)
Update estimate of the geoid height Requires an available hsml and/or lla measurement,...
Definition: traffic_info.c:66
int32_t geoid_height
Definition: traffic_info.c:48
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98