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