Paparazzi UAS  v6.2_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 
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
142  float dvx = vx - acInfoGetVelocityEnu_f(ti_acs[i].ac_id)->x;
143  float dvy = vy - acInfoGetVelocityEnu_f(ti_acs[i].ac_id)->y;
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 }
RA_LEVEL
@ RA_LEVEL
Definition: tcas.h:43
dl_buffer
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:63
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
stateGetHorizontalSpeedDir_f
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
tcas_resolve
enum tcas_resolve tcas_resolve
Definition: tcas.c:39
TCAS_NO_ALARM
#define TCAS_NO_ALARM
Definition: tcas.h:40
tcas_acs_status
struct tcas_ac_status tcas_acs_status[NB_ACS]
Definition: tcas.c:41
TCAS_HUGE_TAU
#define TCAS_HUGE_TAU
Definition: tcas.c:63
tcas.h
Collision avoidance library.
status
uint8_t status
Definition: nps_radio_control_spektrum.c:101
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
parseTcasRA
void parseTcasRA(uint8_t *buf)
Definition: tcas.c:95
tcas_dmod
float tcas_dmod
Definition: tcas.c:36
TCAS_ALIM
#define TCAS_ALIM
Definition: tcas.c:56
tcas_test_direction
static enum tcas_resolve tcas_test_direction(uint8_t id)
Definition: tcas.c:103
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
acInfo::ac_id
uint8_t ac_id
Definition: traffic_info.h:58
TCAS_TAU_RA
#define TCAS_TAU_RA
Definition: tcas.c:48
tcas_ac_status::resolve
enum tcas_resolve resolve
Definition: tcas.h:51
stateGetPositionUtm_f
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
acInfoGetPositionEnu_f
static struct EnuCoor_f * acInfoGetPositionEnu_f(uint8_t ac_id)
Get position in local ENU coordinates (float).
Definition: traffic_info.h:383
TCAS_TA
#define TCAS_TA
Definition: tcas.h:41
callTCAS
void callTCAS(void)
Definition: tcas.c:65
parseTcasResolve
void parseTcasResolve(uint8_t *buf)
Definition: tcas.c:87
tcas_ac_RA
uint8_t tcas_ac_RA
Definition: tcas.c:40
tcas_tau_ta
float tcas_tau_ta
Definition: tcas.c:36
acInfoGetVelocityEnu_f
static struct EnuCoor_f * acInfoGetVelocityEnu_f(uint8_t ac_id)
Get position from ENU coordinates (float).
Definition: traffic_info.h:405
RA_CLIMB
@ RA_CLIMB
Definition: tcas.h:43
tcas_tau_ra
float tcas_tau_ra
Definition: tcas.c:36
tcas_ac_status
Definition: tcas.h:49
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
TCAS_TAU_TA
#define TCAS_TAU_TA
Definition: tcas.c:44
TCAS_IsInside
#define TCAS_IsInside()
Definition: tcas.c:68
tcas_periodic_task_1Hz
void tcas_periodic_task_1Hz(void)
Definition: tcas.c:117
TCAS_DT_MAX
#define TCAS_DT_MAX
Definition: tcas.c:60
TCAS_RA
#define TCAS_RA
Definition: tcas.h:42
tcas_init
void tcas_init(void)
Definition: tcas.c:70
tcas_alt_setpoint
float tcas_alt_setpoint
Definition: tcas.c:35
tcas_periodic_task_4Hz
void tcas_periodic_task_4Hz(void)
Definition: tcas.c:236
nav.h
ti_acs
struct acInfo ti_acs[NB_ACS]
Definition: traffic_info.c:45
stateGetHorizontalSpeedNorm_f
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
ground_alt
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:41
v_ctl_altitude_setpoint
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:88
tcas_ac_status::status
uint8_t status
Definition: tcas.h:50
stateGetSpeedEnu_f
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
tcas_resolve
tcas_resolve
Definition: tcas.h:43
tcas_status
uint8_t tcas_status
Definition: tcas.c:38
tcas_alim
float tcas_alim
Definition: tcas.c:36
NB_ACS
#define NB_ACS
Definition: rssi.c:38
TCAS_DMOD
#define TCAS_DMOD
Definition: tcas.c:52
RA_DESCEND
@ RA_DESCEND
Definition: tcas.h:43
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
nav_altitude
float nav_altitude
Definition: nav.c:308
state.h
ac_id
uint8_t ac_id
Definition: sim_ap.c:47
RA_NONE
@ RA_NONE
Definition: tcas.h:43
gps
struct GpsState gps
global GPS state
Definition: gps.c:69
ti_acs_id
uint8_t ti_acs_id[NB_ACS_ID]
Definition: traffic_info.c:43
Min
#define Min(x, y)
Definition: esc_dshot.c:85
acInfo::itow
uint32_t itow
ms
Definition: traffic_info.h:119