Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
formation.h
Go to the documentation of this file.
1 
7 #ifndef FORMATION_H
8 #define FORMATION_H
9 
10 #include "subsystems/datalink/datalink.h" // dl_buffer
11 #include "generated/airframe.h" // AC_ID
13 
14 #define FORM_MODE_GLOBAL 0
15 #define FORM_MODE_COURSE 1
16 
18 extern float form_prox;
19 extern uint8_t form_mode;
20 extern uint8_t leader_id;
21 
23 
24 struct slot_ {
26  float east;
27  float north;
28  float alt;
29 };
30 
31 extern struct slot_ formation[];
32 
33 extern int formation_init(void);
34 
35 extern int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a);
36 
37 static inline void updateSlot(uint8_t id, float se, float sn, float sa)
38 {
39  formation[ti_acs_id[id]].east = se;
40  formation[ti_acs_id[id]].north = sn;
41  formation[ti_acs_id[id]].alt = sa;
42 }
43 
45 
46 static inline void parseFormationStatus(void)
47 {
48  uint8_t ac_id = DL_FORMATION_STATUS_ac_id(dl_buffer);
49  uint8_t leader = DL_FORMATION_STATUS_leader_id(dl_buffer);
50  uint8_t status = DL_FORMATION_STATUS_status(dl_buffer);
51  if (ac_id == AC_ID) { leader_id = leader; }
52  else if (leader == leader_id) { updateFormationStatus(ac_id, status); }
53  else { updateFormationStatus(ac_id, UNSET); }
54 }
55 
56 static inline void parseFormationSlot(void)
57 {
58  uint8_t ac_id = DL_FORMATION_SLOT_ac_id(dl_buffer);
59  uint8_t mode = DL_FORMATION_SLOT_mode(dl_buffer);
60  float slot_east = DL_FORMATION_SLOT_slot_east(dl_buffer);
61  float slot_north = DL_FORMATION_SLOT_slot_north(dl_buffer);
62  float slot_alt = DL_FORMATION_SLOT_slot_alt(dl_buffer);
63  updateSlot(ac_id, slot_east, slot_north, slot_alt);
64  if (ac_id == leader_id) { form_mode = mode; }
65 }
66 
67 extern int start_formation(void);
68 extern int stop_formation(void);
69 extern int formation_flight(void);
70 extern void formation_pre_call(void);
71 
72 #endif /* FORMATION_H */
float coef_form_speed
Definition: formation.c:25
uint8_t ac_id
Definition: sim_ap.c:45
static void updateFormationStatus(uint8_t id, uint8_t status)
Definition: formation.h:44
float form_prox
Definition: formation.c:23
int formation_init(void)
Definition: formation.c:62
struct slot_ formation[]
Definition: formation.c:32
uint8_t leader_id
Definition: formation.c:29
int stop_formation(void)
Definition: formation.c:106
float east
Definition: formation.h:26
int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a)
Definition: formation.c:80
float coef_form_course
Definition: formation.c:26
uint8_t form_mode
Definition: formation.c:28
Definition: formation.h:22
uint8_t status
static void parseFormationStatus(void)
Definition: formation.h:46
Definition: formation.h:22
float alt
Definition: formation.h:28
static void updateSlot(uint8_t id, float se, float sn, float sa)
Definition: formation.h:37
unsigned char uint8_t
Definition: types.h:14
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
Definition: sonar_bebop.c:69
float north
Definition: formation.h:27
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
enum slot_status status
Definition: formation.h:25
slot_status
Definition: formation.h:22
static void parseFormationSlot(void)
Definition: formation.h:56
float coef_form_alt
Definition: formation.c:27
void formation_pre_call(void)
Definition: formation.c:254
int start_formation(void)
Definition: formation.c:91
uint8_t ti_acs_id[NB_ACS_ID]
Definition: traffic_info.c:43
float coef_form_pos
Definition: formation.c:24
int formation_flight(void)
Definition: formation.c:124