Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
traffic_info.h
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
28 #ifndef TRAFFIC_INFO_H
29 #define TRAFFIC_INFO_H
30 
31 #include "mcu_periph/sys_time.h"
32 #include "math/pprz_geodetic_int.h"
34 #include "modules/gps/gps.h"
35 
36 #ifndef NB_ACS_ID
37 #define NB_ACS_ID 256
38 #endif
39 #ifndef NB_ACS
40 #define NB_ACS 24
41 #endif
42 
47 #define AC_INFO_POS_UTM_I 0
48 #define AC_INFO_POS_LLA_I 1
49 #define AC_INFO_POS_ENU_I 2
50 #define AC_INFO_POS_UTM_F 3
51 #define AC_INFO_POS_LLA_F 4
52 #define AC_INFO_POS_ENU_F 5
53 #define AC_INFO_VEL_ENU_I 6
54 #define AC_INFO_VEL_ENU_F 7
55 #define AC_INFO_VEL_LOCAL_F 8
56 
57 struct acInfo {
65 
71  struct UtmCoor_i utm_pos_i;
72 
78  struct LlaCoor_i lla_pos_i;
79 
84  struct EnuCoor_i enu_pos_i;
85 
90  struct EnuCoor_i enu_vel_i;
91 
97  struct UtmCoor_f utm_pos_f;
98 
104  struct LlaCoor_f lla_pos_f;
105 
109  struct EnuCoor_f enu_pos_f;
110 
114  struct EnuCoor_f enu_vel_f;
115 
116  float course;
117  float gspeed;
118  float climb;
120 };
121 
122 extern uint8_t ti_acs_idx;
123 extern uint8_t ti_acs_id[];
124 extern struct acInfo ti_acs[];
125 
126 extern void traffic_info_init(void);
127 
134 extern bool parse_acinfo_dl(uint8_t *buf);
135 
136 /************************ Set functions ****************************/
137 
150 extern void set_ac_info_utm(uint8_t id, uint32_t utm_east, uint32_t utm_north, uint32_t alt, uint8_t utm_zone,
152 
164 extern void set_ac_info_lla(uint8_t id, int32_t lat, int32_t lon, int32_t alt,
166 
171 static inline void acInfoSetPositionUtm_i(uint8_t ac_id, struct UtmCoor_i *utm_pos)
172 {
173  if (ti_acs_idx < NB_ACS) {
174  if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
177  }
178  UTM_COPY(ti_acs[ti_acs_id[ac_id]].utm_pos_i, *utm_pos);
179  /* clear bits for all position representations and only set the new one */
182  }
183 }
184 
189 static inline void acInfoSetPositionLla_i(uint8_t ac_id, struct LlaCoor_i *lla_pos)
190 {
191  if (ti_acs_idx < NB_ACS) {
192  if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
195  }
196  LLA_COPY(ti_acs[ti_acs_id[ac_id]].lla_pos_i, *lla_pos);
197  /* clear bits for all position representations and only set the new one */
200  }
201 }
202 
207 static inline void acInfoSetPositionEnu_i(uint8_t ac_id, struct EnuCoor_i *enu_pos)
208 {
209  if (ti_acs_idx < NB_ACS) {
210  if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
213  }
215  /* clear bits for all position representations and only set the new one */
218  }
219 }
220 
225 static inline void acInfoSetPositionUtm_f(uint8_t ac_id, struct UtmCoor_f *utm_pos)
226 {
227  if (ti_acs_idx < NB_ACS) {
228  if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
231  }
232  UTM_COPY(ti_acs[ti_acs_id[ac_id]].utm_pos_f, *utm_pos);
233  /* clear bits for all position representations and only set the new one */
236  }
237 }
238 
243 static inline void acInfoSetPositionLla_f(uint8_t ac_id, struct LlaCoor_f *lla_pos)
244 {
245  if (ti_acs_idx < NB_ACS) {
246  if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
249  }
250  LLA_COPY(ti_acs[ti_acs_id[ac_id]].lla_pos_i, *lla_pos);
251  /* clear bits for all position representations and only set the new one */
254  }
255 }
256 
261 static inline void acInfoSetPositionEnu_f(uint8_t ac_id, struct EnuCoor_f *enu_pos)
262 {
263  if (ti_acs_idx < NB_ACS) {
264  if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
267  }
269  /* clear bits for all position representations and only set the new one */
272  }
273 }
274 
279 static inline void acInfoSetVelocityEnu_i(uint8_t ac_id, struct EnuCoor_i *enu_vel)
280 {
281  if (ti_acs_idx < NB_ACS) {
282  if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
285  }
287  /* clear bits for all position representations and only set the new one */
290  }
291 }
292 
297 static inline void acInfoSetVelocityEnu_f(uint8_t ac_id, struct EnuCoor_f *enu_vel)
298 {
299  if (ti_acs_idx < NB_ACS) {
300  if (ac_id > 0 && ti_acs_id[ac_id] == 0) { // new aircraft id
303  }
305  /* clear bits for all position representations and only set the new one */
308  }
309 }
310 
311 
312 /*************** Reference frame conversion functions ***************/
313 
322 
323 /************************ Get functions ****************************/
324 
328 static inline struct UtmCoor_i *acInfoGetPositionUtm_i(uint8_t ac_id)
329 {
330  if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_UTM_I)) {
332  }
333  return &ti_acs[ti_acs_id[ac_id]].utm_pos_i;
334 }
335 
339 static inline struct LlaCoor_i *acInfoGetPositionLla_i(uint8_t ac_id)
340 {
341  if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_LLA_I)) {
343  }
344  return &ti_acs[ti_acs_id[ac_id]].lla_pos_i;
345 }
346 
350 static inline struct EnuCoor_i *acInfoGetPositionEnu_i(uint8_t ac_id)
351 {
352  if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_ENU_I)) {
354  }
355  return &ti_acs[ti_acs_id[ac_id]].enu_pos_i;
356 }
357 
361 static inline struct UtmCoor_f *acInfoGetPositionUtm_f(uint8_t ac_id)
362 {
363  if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_UTM_F)) {
365  }
366  return &ti_acs[ti_acs_id[ac_id]].utm_pos_f;
367 }
368 
372 static inline struct LlaCoor_f *acInfoGetPositionLla_f(uint8_t ac_id)
373 {
374  if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_LLA_F)) {
376  }
377  return &ti_acs[ti_acs_id[ac_id]].lla_pos_f;
378 }
379 
383 static inline struct EnuCoor_f *acInfoGetPositionEnu_f(uint8_t ac_id)
384 {
385  if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_POS_ENU_F)) {
387  }
388  return &ti_acs[ti_acs_id[ac_id]].enu_pos_f;
389 }
390 
394 static inline struct EnuCoor_i *acInfoGetVelocityEnu_i(uint8_t ac_id)
395 {
396  if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_VEL_ENU_I)) {
398  }
399  return &ti_acs[ti_acs_id[ac_id]].enu_vel_i;
400 }
401 
405 static inline struct EnuCoor_f *acInfoGetVelocityEnu_f(uint8_t ac_id)
406 {
407  if (!bit_is_set(ti_acs[ti_acs_id[ac_id]].status, AC_INFO_VEL_ENU_F)) {
409  }
410  return &ti_acs[ti_acs_id[ac_id]].enu_vel_f;
411 }
412 
416 static inline float acInfoGetCourse(uint8_t ac_id)
417 {
418  return ti_acs[ti_acs_id[ac_id]].course;
419 }
420 
424 static inline float acInfoGetGspeed(uint8_t ac_id)
425 {
426  return ti_acs[ti_acs_id[ac_id]].gspeed;
427 }
428 
432 static inline float acInfoGetClimb(uint8_t ac_id)
433 {
434  return ti_acs[ti_acs_id[ac_id]].climb;
435 }
436 
440 static inline uint32_t acInfoGetItow(uint8_t ac_id)
441 {
442  return ti_acs[ti_acs_id[ac_id]].itow;
443 }
444 
447 #endif
static int16_t course[3]
Definition: airspeed_uADC.c:58
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
Device independent GPS code (interface)
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
struct EnuCoor_i enu_vel_i
Velocity in North East Down coordinates.
Definition: traffic_info.h:90
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
struct EnuCoor_f enu_pos_f
Position in North East Down coordinates Units: m.
Definition: traffic_info.h:109
#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
static void acInfoSetPositionLla_i(uint8_t ac_id, struct LlaCoor_i *lla_pos)
Set position from LLA coordinates (int).
Definition: traffic_info.h:189
static float acInfoGetClimb(uint8_t ac_id)
Get vehicle climb speed (float).
Definition: traffic_info.h:432
static void acInfoSetPositionUtm_i(uint8_t ac_id, struct UtmCoor_i *utm_pos)
Set position from UTM coordinates (int).
Definition: traffic_info.h:171
#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
static struct EnuCoor_i * acInfoGetPositionEnu_i(uint8_t ac_id)
Get position in local ENU coordinates (int).
Definition: traffic_info.h:350
static void acInfoSetPositionUtm_f(uint8_t ac_id, struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: traffic_info.h:225
static struct EnuCoor_i * acInfoGetVelocityEnu_i(uint8_t ac_id)
Get position from ENU coordinates (int).
Definition: traffic_info.h:394
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
Definition: traffic_info.h:383
static float acInfoGetCourse(uint8_t ac_id)
Get vehicle course (float).
Definition: traffic_info.h:416
#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 void acInfoSetPositionEnu_f(uint8_t ac_id, struct EnuCoor_f *enu_pos)
Set position from ENU coordinates (float).
Definition: traffic_info.h:261
static struct UtmCoor_i * acInfoGetPositionUtm_i(uint8_t ac_id)
Get position from UTM coordinates (int).
Definition: traffic_info.h:328
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 float acInfoGetGspeed(uint8_t ac_id)
Get vehicle ground speed (float).
Definition: traffic_info.h:424
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
static uint32_t acInfoGetItow(uint8_t ac_id)
Get time of week from latest message (ms).
Definition: traffic_info.h:440
#define AC_INFO_POS_ENU_I
Definition: traffic_info.h:49
static void acInfoSetPositionLla_f(uint8_t ac_id, struct LlaCoor_f *lla_pos)
Set position from LLA coordinates (float).
Definition: traffic_info.h:243
void traffic_info_init(void)
Definition: traffic_info.c:50
struct acInfo ti_acs[]
Definition: traffic_info.c:45
static void acInfoSetVelocityEnu_f(uint8_t ac_id, struct EnuCoor_f *enu_vel)
Set velocity from ENU coordinates (float).
Definition: traffic_info.h:297
uint8_t ti_acs_id[]
Definition: traffic_info.c:43
static void acInfoSetVelocityEnu_i(uint8_t ac_id, struct EnuCoor_i *enu_vel)
Set velocity from ENU coordinates (int).
Definition: traffic_info.h:279
static void acInfoSetPositionEnu_i(uint8_t ac_id, struct EnuCoor_i *enu_pos)
Set position from ENU coordinates (int).
Definition: traffic_info.h:207
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 EnuCoor_f * acInfoGetVelocityEnu_f(uint8_t ac_id)
Get position from ENU coordinates (float).
Definition: traffic_info.h:405
static struct LlaCoor_i * acInfoGetPositionLla_i(uint8_t ac_id)
Get position from LLA coordinates (int).
Definition: traffic_info.h:339
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define LLA_COPY(_pos1, _pos2)
Definition: pprz_geodetic.h:58
#define UTM_COPY(_u1, _u2)
Definition: pprz_geodetic.h:71
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
position in UTM coordinates
uint8_t status
Paparazzi floating point math for geodetic calculations.
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
position in UTM coordinates Units: meters
Paparazzi fixed point math for geodetic calculations.
Architecture independent timing functions.
volatile uint32_t nb_tick
SYS_TIME_TICKS since startup.
Definition: sys_time.h:74
#define NB_ACS
Definition: traffic_info.h:40
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