Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "generated/airframe.h"
30 #include "state.h"
32 #include "subsystems/gps.h"
33 #include "generated/flight_plan.h"
34 
35 #include "messages.h"
37 
40 
45 
46 #ifndef TCAS_TAU_TA // Traffic Advisory
47 #define TCAS_TAU_TA 2*CARROT
48 #endif
49 
50 #ifndef TCAS_TAU_RA // Resolution Advisory
51 #define TCAS_TAU_RA CARROT
52 #endif
53 
54 #ifndef TCAS_DMOD // Distance Modification
55 #define TCAS_DMOD 10.
56 #endif
57 
58 #ifndef TCAS_ALIM // Altitude Limit
59 #define TCAS_ALIM 15.
60 #endif
61 
62 #ifndef TCAS_DT_MAX // ms (lost com and timeout)
63 #define TCAS_DT_MAX 1500
64 #endif
65 
66 #define TCAS_HUGE_TAU 100*TCAS_TAU_TA
67 
68 /* AC is inside the horizontol dmod area and twice the vertical alim separation */
69 #define TCAS_IsInside() ( (ddh < Square(tcas_dmod) && ddv < Square(2*tcas_alim)) ? 1 : 0 )
70 
71 void tcas_init( void ) {
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 
87 static inline enum tcas_resolve tcas_test_direction(uint8_t id) {
88  struct ac_info_ * ac = get_ac_info(id);
89  float dz = ac->alt - stateGetPositionUtm_f()->alt;
90  if (dz > tcas_alim/2) return RA_DESCEND;
91  else if (dz < -tcas_alim/2) return RA_CLIMB;
92  else // AC with the smallest ID descend
93  {
94  if (AC_ID < id) return RA_DESCEND;
95  else return RA_CLIMB;
96  }
97 }
98 
99 
100 /* conflicts detection and monitoring */
102  // no TCAS under security_height
103  if (stateGetPositionUtm_f()->alt < GROUND_ALT + SECURITY_HEIGHT) {
104  uint8_t i;
105  for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM;
106  return;
107  }
108  // test possible conflicts
109  float tau_min = tcas_tau_ta;
110  uint8_t ac_id_close = AC_ID;
111  uint8_t i;
112  float vx = (*stateGetHorizontalSpeedNorm_f()) * sinf((*stateGetHorizontalSpeedDir_f()));
113  float vy = (*stateGetHorizontalSpeedNorm_f()) * cosf((*stateGetHorizontalSpeedDir_f()));
114  for (i = 2; i < NB_ACS; i++) {
115  if (the_acs[i].ac_id == 0) continue; // no AC data
116  uint32_t dt = gps.tow - the_acs[i].itow;
117  if (dt > 3*TCAS_DT_MAX) {
118  tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status
119  continue;
120  }
121  if (dt > TCAS_DT_MAX) continue; // lost com but keep current status
122  float dx = the_acs[i].east - stateGetPositionEnu_f()->x;
123  float dy = the_acs[i].north - stateGetPositionEnu_f()->y;
124  float dz = the_acs[i].alt - stateGetPositionUtm_f()->alt;
125  float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course);
126  float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course);
127  float dvz = stateGetSpeedEnu_f()->z - the_acs[i].climb;
128  float scal = dvx*dx + dvy*dy + dvz*dz;
129  float ddh = dx*dx + dy*dy;
130  float ddv = dz*dz;
131  float tau = TCAS_HUGE_TAU;
132  if (scal > 0.) tau = (ddh + ddv) / scal;
133  // monitor conflicts
134  uint8_t inside = TCAS_IsInside();
135  //enum tcas_resolve test_dir = RA_NONE;
136  switch (tcas_acs_status[i].status) {
137  case TCAS_RA:
138  if (tau >= TCAS_HUGE_TAU && !inside) {
139  tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
141  DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
142  }
143  break;
144  case TCAS_TA:
145  if (tau < tcas_tau_ra || inside) {
146  tcas_acs_status[i].status = TCAS_RA; // TA -> RA
147  // Downlink alert
148  //test_dir = tcas_test_direction(the_acs[i].ac_id);
149  //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);// FIXME only one closest AC ???
150  break;
151  }
152  if (tau > tcas_tau_ta && !inside)
153  tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
155  DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
156  break;
157  case TCAS_NO_ALARM:
158  if (tau < tcas_tau_ta || inside) {
159  tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA
160  // Downlink warning
161  DOWNLINK_SEND_TCAS_TA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
162  }
163  if (tau < tcas_tau_ra || inside) {
164  tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem ?
165  // Downlink alert
166  //test_dir = tcas_test_direction(the_acs[i].ac_id);
167  //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);
168  }
169  break;
170  }
171  // store closest AC
172  if (tau < tau_min) {
173  tau_min = tau;
174  ac_id_close = the_acs[i].ac_id;
175 
176  }
177  }
178  // set current conflict mode
180  ac_id_close = tcas_ac_RA; // keep RA until resolved
181  }
183  // at least one in conflict, deal with closest one
184  if (tcas_status == TCAS_RA) {
185  tcas_ac_RA = ac_id_close;
188  if (ac_resolve != RA_NONE) { // first resolution, no message received
189  if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
190  if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
191  else tcas_resolve = RA_CLIMB;
192  }
193  tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve = RA_LEVEL; // assuming level flight for now
194  }
195  else { // second resolution or message received
196  if (ac_resolve != RA_LEVEL) { // message received
197  if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
198  if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
199  else tcas_resolve = RA_CLIMB;
200  }
201  }
202  else { // no message
203  if (tcas_resolve == RA_CLIMB && the_acs[the_acs_id[tcas_ac_RA]].climb > 1.0) tcas_resolve = RA_DESCEND; // revert resolve
204  else if (tcas_resolve == RA_DESCEND && the_acs[the_acs_id[tcas_ac_RA]].climb < -1.0) tcas_resolve = RA_CLIMB; // revert resolve
205  }
206  }
207  // Downlink alert
208  DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&tcas_ac_RA,&tcas_resolve);
209  }
210  else tcas_ac_RA = AC_ID; // no conflict
211 #ifdef TCAS_DEBUG
212  if (tcas_status == TCAS_RA) DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel, DefaultDevice,&ac_id_close,&tau_min);
213 #endif
214 }
215 
216 
217 /* altitude control loop */
219  // set alt setpoint
220  if (stateGetPositionUtm_f()->alt > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) {
221  struct ac_info_ * ac = get_ac_info(tcas_ac_RA);
222  switch (tcas_resolve) {
223  case RA_CLIMB :
225  break;
226  case RA_DESCEND :
228  break;
229  case RA_LEVEL :
230  case RA_NONE :
232  break;
233  }
234  // Bound alt
235  tcas_alt_setpoint = Max(GROUND_ALT + SECURITY_HEIGHT, tcas_alt_setpoint);
236  }
237  else {
240  }
241 }
status
Definition: anemotaxis.c:10
static enum tcas_resolve tcas_test_direction(uint8_t id)
Definition: tcas.c:87
static float * stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:861
float tcas_alt_setpoint
Definition: tcas.c:38
enum tcas_resolve resolve
Definition: tcas.h:48
#define TCAS_DMOD
Definition: tcas.c:55
uint8_t ac_id
Definition: traffic_info.h:36
#define Min(x, y)
uint8_t the_acs_id[NB_ACS_ID]
Definition: traffic_info.c:34
uint8_t status
Definition: tcas.h:47
struct ac_info_ the_acs[NB_ACS]
Definition: traffic_info.c:35
static const float dt
Definition: tcas.h:40
float north
Definition: traffic_info.h:38
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
float z
in meters
uint8_t tcas_ac_RA
Definition: tcas.c:43
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
struct ac_info_ * get_ac_info(uint8_t id)
Definition: traffic_info.c:44
float y
in meters
float east
Definition: traffic_info.h:37
#define TCAS_RA
Definition: tcas.h:39
uint32_t itow
Definition: traffic_info.h:43
enum tcas_resolve tcas_resolve
Definition: tcas.c:42
float tcas_alim
Definition: tcas.c:39
float alt
Definition: traffic_info.h:40
struct tcas_ac_status tcas_acs_status[NB_ACS]
Definition: tcas.c:44
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
#define TCAS_TAU_TA
Definition: tcas.c:47
float tcas_dmod
Definition: tcas.c:39
#define Max(x, y)
#define TCAS_NO_ALARM
Definition: tcas.h:37
Device independent GPS code (interface)
unsigned long uint32_t
Definition: types.h:18
float x
in meters
float course
Definition: traffic_info.h:39
Definition: tcas.h:40
float tcas_tau_ta
Definition: tcas.c:39
Definition: tcas.h:40
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
#define NB_ACS
Definition: traffic_info.h:33
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:840
float climb
Definition: traffic_info.h:42
Collision avoidance library.
#define TCAS_TA
Definition: tcas.h:38
void tcas_periodic_task_1Hz(void)
Definition: tcas.c:101
#define TCAS_ALIM
Definition: tcas.c:59
#define TCAS_HUGE_TAU
Definition: tcas.c:66
#define TCAS_TAU_RA
Definition: tcas.c:51
#define TCAS_IsInside()
Definition: tcas.c:69
uint8_t ac_id
Definition: jsbsim_hw.c:27
#define TCAS_DT_MAX
Definition: tcas.c:63
uint8_t tcas_status
Definition: tcas.c:41
float tcas_tau_ra
Definition: tcas.c:39
float alt
in meters above WGS84 reference ellipsoid
void tcas_periodic_task_4Hz(void)
Definition: tcas.c:218
struct GpsState gps
global GPS state
Definition: gps.c:41
void tcas_init(void)
Definition: tcas.c:71
float gspeed
Definition: traffic_info.h:41
tcas_resolve
Definition: tcas.h:40