Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
tcas.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2010 ENAC
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
30 #include "multi/tcas.h"
31 #include "generated/airframe.h"
32 #include "estimator.h"
33 #include "subsystems/nav.h"
34 #include "subsystems/gps.h"
35 #include "generated/flight_plan.h"
36 
37 #include "messages.h"
39 
42 
47 
48 #ifndef TCAS_TAU_TA // Traffic Advisory
49 #define TCAS_TAU_TA 2*CARROT
50 #endif
51 
52 #ifndef TCAS_TAU_RA // Resolution Advisory
53 #define TCAS_TAU_RA CARROT
54 #endif
55 
56 #ifndef TCAS_DMOD // Distance Modification
57 #define TCAS_DMOD 10.
58 #endif
59 
60 #ifndef TCAS_ALIM // Altitude Limit
61 #define TCAS_ALIM 15.
62 #endif
63 
64 #ifndef TCAS_DT_MAX // ms (lost com and timeout)
65 #define TCAS_DT_MAX 1500
66 #endif
67 
68 #define TCAS_HUGE_TAU 100*TCAS_TAU_TA
69 
70 /* AC is inside the horizontol dmod area and twice the vertical alim separation */
71 #define TCAS_IsInside() ( (ddh < Square(tcas_dmod) && ddv < Square(2*tcas_alim)) ? 1 : 0 )
72 
73 void tcas_init( void ) {
74  tcas_alt_setpoint = GROUND_ALT + SECURITY_HEIGHT;
81  tcas_ac_RA = AC_ID;
82  uint8_t i;
83  for (i = 0; i < NB_ACS; i++) {
86  }
87 }
88 
89 static inline enum tcas_resolve tcas_test_direction(uint8_t id) {
90  struct ac_info_ * ac = get_ac_info(id);
91  float dz = ac->alt - estimator_z;
92  if (dz > tcas_alim/2) return RA_DESCEND;
93  else if (dz < -tcas_alim/2) return RA_CLIMB;
94  else // AC with the smallest ID descend
95  {
96  if (AC_ID < id) return RA_DESCEND;
97  else return RA_CLIMB;
98  }
99 }
100 
101 
102 /* conflicts detection and monitoring */
104  // no TCAS under security_height
105  if (estimator_z < GROUND_ALT + SECURITY_HEIGHT) {
106  uint8_t i;
107  for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM;
108  return;
109  }
110  // test possible conflicts
111  float tau_min = tcas_tau_ta;
112  uint8_t ac_id_close = AC_ID;
113  uint8_t i;
114  float vx = estimator_hspeed_mod * sinf(estimator_hspeed_dir);
115  float vy = estimator_hspeed_mod * cosf(estimator_hspeed_dir);
116  for (i = 2; i < NB_ACS; i++) {
117  if (the_acs[i].ac_id == 0) continue; // no AC data
118  uint32_t dt = gps.tow - the_acs[i].itow;
119  if (dt > 3*TCAS_DT_MAX) {
120  tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status
121  continue;
122  }
123  if (dt > TCAS_DT_MAX) continue; // lost com but keep current status
124  float dx = the_acs[i].east - estimator_x;
125  float dy = the_acs[i].north - estimator_y;
126  float dz = the_acs[i].alt - estimator_z;
127  float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course);
128  float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course);
129  float dvz = estimator_z_dot - the_acs[i].climb;
130  float scal = dvx*dx + dvy*dy + dvz*dz;
131  float ddh = dx*dx + dy*dy;
132  float ddv = dz*dz;
133  float tau = TCAS_HUGE_TAU;
134  if (scal > 0.) tau = (ddh + ddv) / scal;
135  // monitor conflicts
136  uint8_t inside = TCAS_IsInside();
137  //enum tcas_resolve test_dir = RA_NONE;
138  switch (tcas_acs_status[i].status) {
139  case TCAS_RA:
140  if (tau >= TCAS_HUGE_TAU && !inside) {
141  tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
143  DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
144  }
145  break;
146  case TCAS_TA:
147  if (tau < tcas_tau_ra || inside) {
148  tcas_acs_status[i].status = TCAS_RA; // TA -> RA
149  // Downlink alert
150  //test_dir = tcas_test_direction(the_acs[i].ac_id);
151  //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);// FIXME only one closest AC ???
152  break;
153  }
154  if (tau > tcas_tau_ta && !inside)
155  tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
157  DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
158  break;
159  case TCAS_NO_ALARM:
160  if (tau < tcas_tau_ta || inside) {
161  tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA
162  // Downlink warning
163  DOWNLINK_SEND_TCAS_TA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
164  }
165  if (tau < tcas_tau_ra || inside) {
166  tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem ?
167  // Downlink alert
168  //test_dir = tcas_test_direction(the_acs[i].ac_id);
169  //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);
170  }
171  break;
172  }
173  // store closest AC
174  if (tau < tau_min) {
175  tau_min = tau;
176  ac_id_close = the_acs[i].ac_id;
177 
178  }
179  }
180  // set current conflict mode
182  ac_id_close = tcas_ac_RA; // keep RA until resolved
183  }
185  // at least one in conflict, deal with closest one
186  if (tcas_status == TCAS_RA) {
187  tcas_ac_RA = ac_id_close;
190  if (ac_resolve != RA_NONE) { // first resolution, no message received
191  if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
192  if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
193  else tcas_resolve = RA_CLIMB;
194  }
195  tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve = RA_LEVEL; // assuming level flight for now
196  }
197  else { // second resolution or message received
198  if (ac_resolve != RA_LEVEL) { // message received
199  if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
200  if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
201  else tcas_resolve = RA_CLIMB;
202  }
203  }
204  else { // no message
205  if (tcas_resolve == RA_CLIMB && the_acs[the_acs_id[tcas_ac_RA]].climb > 1.0) tcas_resolve = RA_DESCEND; // revert resolve
206  else if (tcas_resolve == RA_DESCEND && the_acs[the_acs_id[tcas_ac_RA]].climb < -1.0) tcas_resolve = RA_CLIMB; // revert resolve
207  }
208  }
209  // Downlink alert
210  DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&tcas_ac_RA,&tcas_resolve);
211  }
212  else tcas_ac_RA = AC_ID; // no conflict
213 #ifdef TCAS_DEBUG
214  if (tcas_status == TCAS_RA) DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel, DefaultDevice,&ac_id_close,&tau_min);
215 #endif
216 }
217 
218 
219 /* altitude control loop */
221  // set alt setpoint
222  if (estimator_z > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) {
223  struct ac_info_ * ac = get_ac_info(tcas_ac_RA);
224  switch (tcas_resolve) {
225  case RA_CLIMB :
227  break;
228  case RA_DESCEND :
230  break;
231  case RA_LEVEL :
232  case RA_NONE :
234  break;
235  }
236  // Bound alt
237  tcas_alt_setpoint = Max(GROUND_ALT + SECURITY_HEIGHT, tcas_alt_setpoint);
238  }
239  else {
242  }
243 }
status
Definition: anemotaxis.c:10
static enum tcas_resolve tcas_test_direction(uint8_t id)
Definition: tcas.c:89
float tcas_alt_setpoint
Definition: tcas.c:40
enum tcas_resolve resolve
Definition: tcas.h:50
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
#define TCAS_DMOD
Definition: tcas.c:57
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:49
struct ac_info_ the_acs[NB_ACS]
Definition: traffic_info.c:35
float estimator_z_dot
Definition: estimator.c:46
Definition: tcas.h:42
float north
Definition: traffic_info.h:38
float estimator_y
north position in meters
Definition: estimator.c:43
uint8_t tcas_ac_RA
Definition: tcas.c:45
struct ac_info_ * get_ac_info(uint8_t id)
Definition: traffic_info.c:44
float estimator_hspeed_dir
direction of horizontal ground speed in rad (CW/North)
Definition: estimator.c:65
float east
Definition: traffic_info.h:37
#define TCAS_RA
Definition: tcas.h:41
uint32_t itow
Definition: traffic_info.h:43
enum tcas_resolve tcas_resolve
Definition: tcas.c:44
float tcas_alim
Definition: tcas.c:41
float alt
Definition: traffic_info.h:40
struct tcas_ac_status tcas_acs_status[NB_ACS]
Definition: tcas.c:46
uint32_t tow
GPS time of week in ms.
Definition: gps.h:79
#define TCAS_TAU_TA
Definition: tcas.c:49
float tcas_dmod
Definition: tcas.c:41
#define Max(x, y)
#define TCAS_NO_ALARM
Definition: tcas.h:39
Device independent GPS code (interface)
unsigned long uint32_t
Definition: types.h:18
float course
Definition: traffic_info.h:39
float estimator_x
east position in meters
Definition: estimator.c:42
Definition: tcas.h:42
float tcas_tau_ta
Definition: tcas.c:41
Definition: tcas.h:42
#define NB_ACS
Definition: traffic_info.h:33
unsigned char uint8_t
Definition: types.h:14
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
float climb
Definition: traffic_info.h:42
Collision avoidance library.
#define TCAS_TA
Definition: tcas.h:40
void tcas_periodic_task_1Hz(void)
Definition: tcas.c:103
State estimation, fusioning sensors.
#define TCAS_ALIM
Definition: tcas.c:61
#define TCAS_HUGE_TAU
Definition: tcas.c:68
#define TCAS_TAU_RA
Definition: tcas.c:53
#define TCAS_IsInside()
Definition: tcas.c:71
uint8_t ac_id
Definition: jsbsim_hw.c:28
#define TCAS_DT_MAX
Definition: tcas.c:65
uint8_t tcas_status
Definition: tcas.c:43
float tcas_tau_ra
Definition: tcas.c:41
void tcas_periodic_task_4Hz(void)
Definition: tcas.c:220
struct GpsState gps
global GPS state
Definition: gps.c:31
void tcas_init(void)
Definition: tcas.c:73
float gspeed
Definition: traffic_info.h:41
tcas_resolve
Definition: tcas.h:42