Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
tcas.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 ENAC
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  */
22 
28 #include "multi/tcas.h"
29 #include "state.h"
31 #include "generated/flight_plan.h" // SECURITY_HEIGHT
32 
34 
37 
42 
43 #ifndef TCAS_TAU_TA // Traffic Advisory
44 #define TCAS_TAU_TA 2*CARROT
45 #endif
46 
47 #ifndef TCAS_TAU_RA // Resolution Advisory
48 #define TCAS_TAU_RA CARROT
49 #endif
50 
51 #ifndef TCAS_DMOD // Distance Modification
52 #define TCAS_DMOD 10.
53 #endif
54 
55 #ifndef TCAS_ALIM // Altitude Limit
56 #define TCAS_ALIM 15.
57 #endif
58 
59 #ifndef TCAS_DT_MAX // ms (lost com and timeout)
60 #define TCAS_DT_MAX 1500
61 #endif
62 
63 #define TCAS_HUGE_TAU 100*TCAS_TAU_TA
64 
66 
67 /* AC is inside the horizontol dmod area and twice the vertical alim separation */
68 #define TCAS_IsInside() ( (ddh < Square(tcas_dmod) && ddv < Square(2*tcas_alim)) ? 1 : 0 )
69 
70 void tcas_init(void)
71 {
72  tcas_alt_setpoint = ground_alt + SECURITY_HEIGHT;
79  tcas_ac_RA = AC_ID;
80  uint8_t i;
81  for (i = 0; i < NB_ACS; i++) {
84  }
85 }
86 
88 {
89  if (DL_TCAS_RESOLVE_ac_id(buf) == AC_ID) {
90  uint8_t ac_id_conflict = DL_TCAS_RESOLVE_ac_id_conflict(buf);
91  tcas_acs_status[ti_acs_id[ac_id_conflict]].resolve = DL_TCAS_RESOLVE_resolve(buf);
92  }
93 }
94 
95 void parseTcasRA(uint8_t *buf)
96 {
97  if (DL_TCAS_RA_ac_id(buf) == AC_ID && SenderIdOfPprzMsg(buf) != AC_ID) {
98  uint8_t ac_id_conflict = SenderIdOfPprzMsg(dl_buffer);
99  tcas_acs_status[ti_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(buf);
100  }
101 }
102 
103 static inline enum tcas_resolve tcas_test_direction(uint8_t id)
104 {
105  struct EnuCoor_f *ac = acInfoGetPositionEnu_f(id);
106  float dz = ac->z - stateGetPositionEnu_f()->z;
107  if (dz > tcas_alim / 2) { return RA_DESCEND; }
108  else if (dz < -tcas_alim / 2) { return RA_CLIMB; }
109  else { // AC with the smallest ID descend
110  if (AC_ID < id) { return RA_DESCEND; }
111  else { return RA_CLIMB; }
112  }
113 }
114 
115 
116 /* conflicts detection and monitoring */
118 {
119  // no TCAS under security_height
120  if (stateGetPositionUtm_f()->alt < ground_alt + SECURITY_HEIGHT) {
121  uint8_t i;
122  for (i = 0; i < NB_ACS; i++) { tcas_acs_status[i].status = TCAS_NO_ALARM; }
123  return;
124  }
125  // test possible conflicts
126  float tau_min = tcas_tau_ta;
127  uint8_t ac_id_close = AC_ID;
128  uint8_t i;
131  for (i = 2; i < NB_ACS; i++) {
132  if (ti_acs[i].ac_id == 0) { continue; } // no AC data
133  uint32_t dt = gps.tow - ti_acs[i].itow;
134  if (dt > 3 * TCAS_DT_MAX) {
135  tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status
136  continue;
137  }
138  if (dt > TCAS_DT_MAX) { continue; } // lost com but keep current status
139  float dx = acInfoGetPositionEnu_f(ti_acs[i].ac_id)->x - stateGetPositionEnu_f()->x;
140  float dy = acInfoGetPositionEnu_f(ti_acs[i].ac_id)->y - stateGetPositionEnu_f()->y;
141  float dz = acInfoGetPositionEnu_f(ti_acs[i].ac_id)->z - stateGetPositionEnu_f()->z;
142  float dvx = vx - acInfoGetVelocityEnu_f(ti_acs[i].ac_id)->x;
143  float dvy = vy - acInfoGetVelocityEnu_f(ti_acs[i].ac_id)->y;
144  float dvz = stateGetSpeedEnu_f()->z - acInfoGetVelocityEnu_f(ti_acs[i].ac_id)->z;
145  float scal = dvx * dx + dvy * dy + dvz * dz;
146  float ddh = dx * dx + dy * dy;
147  float ddv = dz * dz;
148  float tau = TCAS_HUGE_TAU;
149  if (scal > 0.) { tau = (ddh + ddv) / scal; }
150  // monitor conflicts
151  uint8_t inside = TCAS_IsInside();
152  //enum tcas_resolve test_dir = RA_NONE;
153  switch (tcas_acs_status[i].status) {
154  case TCAS_RA:
155  if (tau >= TCAS_HUGE_TAU && !inside) {
156  tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
158  DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice, &(ti_acs[i].ac_id));
159  }
160  break;
161  case TCAS_TA:
162  if (tau < tcas_tau_ra || inside) {
163  tcas_acs_status[i].status = TCAS_RA; // TA -> RA
164  // Downlink alert
165  //test_dir = tcas_test_direction(ti_acs[i].ac_id);
166  //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(ti_acs[i].ac_id),&test_dir);// FIXME only one closest AC ???
167  break;
168  }
169  if (tau > tcas_tau_ta && !inside) {
170  tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
171  }
173  DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice, &(ti_acs[i].ac_id));
174  break;
175  case TCAS_NO_ALARM:
176  if (tau < tcas_tau_ta || inside) {
177  tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA
178  // Downlink warning
179  DOWNLINK_SEND_TCAS_TA(DefaultChannel, DefaultDevice, &(ti_acs[i].ac_id));
180  }
181  if (tau < tcas_tau_ra || inside) {
182  tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem ?
183  // Downlink alert
184  //test_dir = tcas_test_direction(ti_acs[i].ac_id);
185  //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(ti_acs[i].ac_id),&test_dir);
186  }
187  break;
188  default:
189  break;
190  }
191  // store closest AC
192  if (tau < tau_min) {
193  tau_min = tau;
194  ac_id_close = ti_acs[i].ac_id;
195 
196  }
197  }
198  // set current conflict mode
200  ac_id_close = tcas_ac_RA; // keep RA until resolved
201  }
203  // at least one in conflict, deal with closest one
204  if (tcas_status == TCAS_RA) {
205  tcas_ac_RA = ac_id_close;
208  if (ac_resolve != RA_NONE) { // first resolution, no message received
209  if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
210  if (AC_ID < tcas_ac_RA) { tcas_resolve = RA_DESCEND; }
211  else { tcas_resolve = RA_CLIMB; }
212  }
213  tcas_acs_status[ti_acs_id[tcas_ac_RA]].resolve = RA_LEVEL; // assuming level flight for now
214  } else { // second resolution or message received
215  if (ac_resolve != RA_LEVEL) { // message received
216  if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
217  if (AC_ID < tcas_ac_RA) { tcas_resolve = RA_DESCEND; }
218  else { tcas_resolve = RA_CLIMB; }
219  }
220  } else { // no message
221  if (tcas_resolve == RA_CLIMB && ti_acs[ti_acs_id[tcas_ac_RA]].climb > 1.0) { tcas_resolve = RA_DESCEND; } // revert resolve
222  else if (tcas_resolve == RA_DESCEND && ti_acs[ti_acs_id[tcas_ac_RA]].climb < -1.0) { tcas_resolve = RA_CLIMB; } // revert resolve
223  }
224  }
225  // Downlink alert
226  uint8_t resolve = tcas_resolve;
227  DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice, &tcas_ac_RA, &resolve);
228  } else { tcas_ac_RA = AC_ID; } // no conflict
229 #ifdef TCAS_DEBUG
230  if (tcas_status == TCAS_RA) { DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel, DefaultDevice, &ac_id_close, &tau_min); }
231 #endif
232 }
233 
234 
235 /* altitude control loop */
237 {
238  // set alt setpoint
239  if (stateGetPositionUtm_f()->alt > ground_alt + SECURITY_HEIGHT && tcas_status == TCAS_RA) {
241  switch (tcas_resolve) {
242  case RA_CLIMB :
244  break;
245  case RA_DESCEND :
247  break;
248  case RA_LEVEL :
249  case RA_NONE :
251  break;
252  default:
253  break;
254  }
255  // Bound alt
256  tcas_alt_setpoint = Max(ground_alt + SECURITY_HEIGHT, tcas_alt_setpoint);
257  } else {
260  }
261 }
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:41
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:88
#define Min(x, y)
Definition: esc_dshot.c:109
struct GpsState gps
global GPS state
Definition: gps.c:69
uint32_t tow
GPS time of week in ms.
Definition: gps.h:108
uint8_t ac_id
Definition: traffic_info.h:58
uint32_t itow
ms
Definition: traffic_info.h:119
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
Definition: traffic_info.h:383
struct acInfo ti_acs[NB_ACS]
Definition: traffic_info.c:45
uint8_t ti_acs_id[NB_ACS_ID]
Definition: traffic_info.c:43
static struct EnuCoor_f * acInfoGetVelocityEnu_f(uint8_t ac_id)
Get position from ENU coordinates (float).
Definition: traffic_info.h:405
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:63
float nav_altitude
Definition: nav.c:307
Fixedwing Navigation library.
uint8_t status
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
#define NB_ACS
Definition: rssi.c:38
API to get/set the generic vehicle states.
#define TCAS_HUGE_TAU
Definition: tcas.c:63
float tcas_tau_ra
Definition: tcas.c:36
float tcas_alim
Definition: tcas.c:36
float tcas_dmod
Definition: tcas.c:36
static enum tcas_resolve tcas_test_direction(uint8_t id)
Definition: tcas.c:103
void tcas_init(void)
Definition: tcas.c:70
struct tcas_ac_status tcas_acs_status[NB_ACS]
Definition: tcas.c:41
#define TCAS_DMOD
Definition: tcas.c:52
uint8_t tcas_ac_RA
Definition: tcas.c:40
#define TCAS_TAU_TA
Definition: tcas.c:44
float tcas_alt_setpoint
Definition: tcas.c:35
void callTCAS(void)
Definition: tcas.c:65
void parseTcasResolve(uint8_t *buf)
Definition: tcas.c:87
#define TCAS_ALIM
Definition: tcas.c:56
void tcas_periodic_task_4Hz(void)
Definition: tcas.c:236
enum tcas_resolve tcas_resolve
Definition: tcas.c:39
float tcas_tau_ta
Definition: tcas.c:36
#define TCAS_TAU_RA
Definition: tcas.c:48
#define TCAS_IsInside()
Definition: tcas.c:68
void tcas_periodic_task_1Hz(void)
Definition: tcas.c:117
uint8_t tcas_status
Definition: tcas.c:38
void parseTcasRA(uint8_t *buf)
Definition: tcas.c:95
#define TCAS_DT_MAX
Definition: tcas.c:60
Collision avoidance library.
tcas_resolve
Definition: tcas.h:43
@ RA_DESCEND
Definition: tcas.h:43
@ RA_LEVEL
Definition: tcas.h:43
@ RA_NONE
Definition: tcas.h:43
@ RA_CLIMB
Definition: tcas.h:43
#define TCAS_TA
Definition: tcas.h:41
enum tcas_resolve resolve
Definition: tcas.h:51
uint8_t status
Definition: tcas.h:50
#define TCAS_NO_ALARM
Definition: tcas.h:40
#define TCAS_RA
Definition: tcas.h:42
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98