[paparazzi-commits] [6226] move tcas (collision avoidence system) to modules

[Gautier Hattenberger]

Revision: 6226
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6226
Author:   gautier
Date:     2010-10-25 08:58:08 +0000 (Mon, 25 Oct 2010)
Log Message:
-----------
move tcas (collision avoidence system) to modules

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/datalink.c

Added Paths:
-----------
    paparazzi3/trunk/conf/modules/tcas.xml
    paparazzi3/trunk/sw/airborne/modules/multi/tcas.c
    paparazzi3/trunk/sw/airborne/modules/multi/tcas.h

Removed Paths:
-------------
    paparazzi3/trunk/sw/airborne/tcas.c
    paparazzi3/trunk/sw/airborne/tcas.h

Added: paparazzi3/trunk/conf/modules/tcas.xml
===================================================================
--- paparazzi3/trunk/conf/modules/tcas.xml                              (rev 0)
+++ paparazzi3/trunk/conf/modules/tcas.xml      2010-10-25 08:58:08 UTC (rev 
6226)
@@ -0,0 +1,16 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="tcas" dir="multi">
+  <header>
+    <file name="tcas.h"/>
+  </header>
+  <init fun="tcas_init()"/>
+  <periodic fun="tcas_periodic_task_1Hz()" freq="1"/>
+  <periodic fun="tcas_periodic_task_4Hz()" freq="4"/>
+  <datalink message="TCAS_RESOLVE" fun="ParseTcasResolve()"/>
+  <makefile>
+    <file name="tcas.c"/>
+    <flag name="TCAS"/>
+  </makefile>
+</module>
+

Modified: paparazzi3/trunk/sw/airborne/datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/datalink.c     2010-10-25 08:54:38 UTC (rev 
6225)
+++ paparazzi3/trunk/sw/airborne/datalink.c     2010-10-25 08:58:08 UTC (rev 
6226)
@@ -39,10 +39,6 @@
 #include "traffic_info.h"
 #endif // TRAFFIC_INFO
 
-#ifdef TCAS
-#include "tcas.h"
-#endif
-
 #ifdef USE_JOYSTICK
 #include "joystick.h"
 #endif
@@ -137,12 +133,6 @@
     SEND_NAVIGATION(DefaultChannel);
   } else
 #endif /** NAV */
-#ifdef TCAS
-  if (msg_id == DL_TCAS_RESOLVE && DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID) {
-    uint8_t ac_id_conflict = DL_TCAS_RESOLVE_ac_id_conflict(dl_buffer);
-    tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = 
DL_TCAS_RESOLVE_resolve(dl_buffer);
-  } else
-#endif
 #ifdef WIND_INFO
   if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
     wind_east = DL_WIND_INFO_east(dl_buffer);

Copied: paparazzi3/trunk/sw/airborne/modules/multi/tcas.c (from rev 6224, 
paparazzi3/trunk/sw/airborne/tcas.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/multi/tcas.c                           
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/multi/tcas.c   2010-10-25 08:58:08 UTC 
(rev 6226)
@@ -0,0 +1,243 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2010 ENAC
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ *
+ */
+
+/** \file tcas.c
+ *  \brief Collision avoidance library
+ * 
+ */
+
+#include "multi/tcas.h"
+#include "airframe.h"
+#include "estimator.h"
+#include "nav.h"
+#include "gps.h"
+#include "flight_plan.h"
+
+#include "messages.h"
+#include "downlink.h"
+
+float tcas_alt_setpoint;
+float tcas_tau_ta, tcas_tau_ra, tcas_dmod, tcas_alim;
+
+uint8_t tcas_status;
+enum tcas_resolve tcas_resolve;
+uint8_t tcas_ac_RA;
+struct tcas_ac_status tcas_acs_status[NB_ACS];
+
+#ifndef TCAS_TAU_TA     // Traffic Advisory
+#define TCAS_TAU_TA 2*CARROT
+#endif
+
+#ifndef TCAS_TAU_RA     // Resolution Advisory
+#define TCAS_TAU_RA CARROT
+#endif
+
+#ifndef TCAS_DMOD       // Distance Modification
+#define TCAS_DMOD 10.
+#endif
+
+#ifndef TCAS_ALIM       // Altitude Limit
+#define TCAS_ALIM 15.
+#endif
+
+#ifndef TCAS_DT_MAX     // ms (lost com and timeout)
+#define TCAS_DT_MAX 1500
+#endif
+
+#define TCAS_HUGE_TAU 100*TCAS_TAU_TA
+
+/* AC is inside the horizontol dmod area and twice the vertical alim 
separation */
+#define TCAS_IsInside() ( (ddh < Square(tcas_dmod) && ddv < 
Square(2*tcas_alim)) ? 1 : 0 )
+
+void tcas_init( void ) {
+  tcas_alt_setpoint = GROUND_ALT + SECURITY_HEIGHT;
+  tcas_tau_ta = TCAS_TAU_TA;
+  tcas_tau_ra = TCAS_TAU_RA;
+  tcas_dmod = TCAS_DMOD;
+  tcas_alim = TCAS_ALIM;
+  tcas_status = TCAS_NO_ALARM;
+  tcas_resolve = RA_NONE;
+  tcas_ac_RA = AC_ID;
+  uint8_t i;
+  for (i = 0; i < NB_ACS; i++) {
+    tcas_acs_status[i].status = TCAS_NO_ALARM;
+    tcas_acs_status[i].resolve = RA_NONE;
+  }
+}
+
+static inline enum tcas_resolve tcas_test_direction(uint8_t id) {
+  struct ac_info_ * ac = get_ac_info(id);
+  float dz = ac->alt - estimator_z;
+  if (dz > tcas_alim) return RA_DESCEND;
+  else if (dz < -tcas_alim) return RA_CLIMB;
+  else // AC with the smallest ID descend
+  {
+    if (AC_ID < id) return RA_DESCEND;
+    else return RA_CLIMB;
+  }
+}
+
+
+/* conflicts detection and monitoring */
+void tcas_periodic_task_1Hz( void ) {
+  // no TCAS under security_height
+  if (estimator_z < GROUND_ALT + SECURITY_HEIGHT) {
+    uint8_t i;
+    for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM;
+    return;
+  }
+  // test possible conflicts
+  float tau_min = tcas_tau_ta;
+  uint8_t ac_id_close = AC_ID;
+  uint8_t i;
+  float vx = estimator_hspeed_mod * sinf(estimator_hspeed_dir);
+  float vy = estimator_hspeed_mod * cosf(estimator_hspeed_dir);
+  for (i = 2; i < NB_ACS; i++) {
+    if (the_acs[i].ac_id == 0) continue; // no AC data
+    uint32_t dt = gps_itow - the_acs[i].itow;
+    if (dt > 3*TCAS_DT_MAX) {
+      tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status
+      continue;
+    }
+    if (dt > TCAS_DT_MAX) continue; // lost com but keep current status
+    float dx = the_acs[i].east - estimator_x;
+    float dy = the_acs[i].north - estimator_y;
+    float dz = the_acs[i].alt - estimator_z;
+    float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course);
+    float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course);
+    float dvz = estimator_z_dot - the_acs[i].climb;
+    float scal = dvx*dx + dvy*dy + dvz*dz;
+    float ddh = dx*dx + dy*dy;
+    float ddv = dz*dz;
+    float tau = TCAS_HUGE_TAU;
+    if (scal > 0.) tau = (ddh + ddv) / scal;
+    // monitor conflicts
+    uint8_t inside = TCAS_IsInside();
+    //enum tcas_resolve test_dir = RA_NONE;
+    switch (tcas_acs_status[i].status) {
+      case TCAS_RA:
+        if (tau >= TCAS_HUGE_TAU && !inside) {
+          tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now 
resolved
+          tcas_acs_status[i].resolve = RA_NONE;
+          DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel,&(the_acs[i].ac_id));
+        }
+        break;
+      case TCAS_TA:
+        if (tau < tcas_tau_ra || inside) {
+          tcas_acs_status[i].status = TCAS_RA; // TA -> RA
+          // Downlink alert
+          //test_dir = tcas_test_direction(the_acs[i].ac_id);
+          
//DOWNLINK_SEND_TCAS_RA(DefaultChannel,&(the_acs[i].ac_id),&test_dir);// FIXME 
only one closest AC ???
+          break;
+        }
+        if (tau > tcas_tau_ta && !inside)
+          tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now 
resolved
+          tcas_acs_status[i].resolve = RA_NONE;
+          DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel,&(the_acs[i].ac_id));
+        break;
+      case TCAS_NO_ALARM:
+        if (tau < tcas_tau_ta || inside) {
+          tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA
+          // Downlink warning
+          DOWNLINK_SEND_TCAS_TA(DefaultChannel,&(the_acs[i].ac_id));
+        }
+        if (tau < tcas_tau_ra || inside) {
+          tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem 
?
+          // Downlink alert
+          //test_dir = tcas_test_direction(the_acs[i].ac_id);
+          
//DOWNLINK_SEND_TCAS_RA(DefaultChannel,&(the_acs[i].ac_id),&test_dir);
+        }
+        break;
+    }
+    // store closest AC
+    if (tau < tau_min) {
+      tau_min = tau;
+      ac_id_close = the_acs[i].ac_id;
+
+    }
+  }
+  // set current conflict mode
+  if (tcas_status == TCAS_RA && tcas_ac_RA != AC_ID && 
tcas_acs_status[the_acs_id[tcas_ac_RA]].status == TCAS_RA) {
+    ac_id_close = tcas_ac_RA; // keep RA until resolved
+  }
+  tcas_status = tcas_acs_status[the_acs_id[ac_id_close]].status;
+  // at least one in conflict, deal with closest one
+  if (tcas_status == TCAS_RA) {
+    tcas_ac_RA = ac_id_close;
+    tcas_resolve = tcas_test_direction(tcas_ac_RA);
+    uint8_t ac_resolve = tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve;
+    if (ac_resolve != RA_NONE) { // first resolution, no message received
+      if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
+        if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
+        else tcas_resolve = RA_CLIMB;
+      }
+      tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve = RA_LEVEL; // assuming 
level flight for now
+    }
+    else { // second resolution or message received
+      if (ac_resolve != RA_LEVEL) { // message received
+        if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
+          if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
+          else tcas_resolve = RA_CLIMB;
+        }
+      }
+      else { // no message
+        if (tcas_resolve == RA_CLIMB && the_acs[the_acs_id[tcas_ac_RA]].climb 
> 1.0) tcas_resolve = RA_DESCEND; // revert resolve
+        else if (tcas_resolve == RA_DESCEND && 
the_acs[the_acs_id[tcas_ac_RA]].climb < -1.0) tcas_resolve = RA_CLIMB; // 
revert resolve
+      }
+    }
+    // Downlink alert
+    DOWNLINK_SEND_TCAS_RA(DefaultChannel,&tcas_ac_RA,&tcas_resolve);
+  }
+  else tcas_ac_RA = AC_ID; // no conflict
+#ifdef TCAS_DEBUG
+  if (tcas_status == TCAS_RA) 
DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel,&ac_id_close,&tau_min);
+#endif
+}
+
+
+/* altitude control loop */
+void tcas_periodic_task_4Hz( void ) {
+  // set alt setpoint
+  if (estimator_z > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) {
+    struct ac_info_ * ac = get_ac_info(tcas_ac_RA);
+    switch (tcas_resolve) {
+      case RA_CLIMB :
+        tcas_alt_setpoint = Max(nav_altitude, ac->alt + tcas_alim);
+        break;
+      case RA_DESCEND :
+        tcas_alt_setpoint = Min(nav_altitude, ac->alt - tcas_alim);
+        break;
+      case RA_LEVEL :
+      case RA_NONE :
+        tcas_alt_setpoint = nav_altitude;
+        break;
+    }
+    // Bound alt
+    tcas_alt_setpoint = Max(GROUND_ALT + SECURITY_HEIGHT, tcas_alt_setpoint);
+  }
+  else {
+    tcas_alt_setpoint = nav_altitude;
+    tcas_resolve = RA_NONE;
+  }
+}

Copied: paparazzi3/trunk/sw/airborne/modules/multi/tcas.h (from rev 6224, 
paparazzi3/trunk/sw/airborne/tcas.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/multi/tcas.h                           
(rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/multi/tcas.h   2010-10-25 08:58:08 UTC 
(rev 6226)
@@ -0,0 +1,68 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2010 ENAC
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ *
+ */
+
+/** \file tcas.h
+ *  \brief Collision avoidance library
+ * 
+ */
+
+#ifndef TCAS_H
+#define TCAS_H
+
+#include "std.h"
+#include "traffic_info.h"
+
+extern float tcas_alt_setpoint;
+extern float tcas_tau_ta, tcas_tau_ra, tcas_dmod, tcas_alim;
+
+#define TCAS_NO_ALARM 0
+#define TCAS_TA 1
+#define TCAS_RA 2
+enum tcas_resolve { RA_NONE, RA_LEVEL, RA_CLIMB, RA_DESCEND };
+
+extern uint8_t tcas_status;
+extern enum tcas_resolve tcas_resolve;
+extern uint8_t tcas_ac_RA;
+
+struct tcas_ac_status {
+  uint8_t status;
+  enum tcas_resolve resolve;
+};
+
+extern struct tcas_ac_status tcas_acs_status[NB_ACS];
+
+extern void tcas_init( void );
+extern void tcas_periodic_task_1Hz( void );
+extern void tcas_periodic_task_4Hz( void );
+
+#define CallTCAS() { if (tcas_status == TCAS_RA) v_ctl_altitude_setpoint = 
tcas_alt_setpoint; }
+
+#define ParseTcasResolve() { \
+  if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID) { \
+    uint8_t ac_id_conflict = DL_TCAS_RESOLVE_ac_id_conflict(dl_buffer); \
+    tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = 
DL_TCAS_RESOLVE_resolve(dl_buffer); \
+  } \
+}
+
+#endif // TCAS

Deleted: paparazzi3/trunk/sw/airborne/tcas.c
===================================================================
--- paparazzi3/trunk/sw/airborne/tcas.c 2010-10-25 08:54:38 UTC (rev 6225)
+++ paparazzi3/trunk/sw/airborne/tcas.c 2010-10-25 08:58:08 UTC (rev 6226)
@@ -1,243 +0,0 @@
-/*
- * Paparazzi mcu0 $Id$
- *  
- * Copyright (C) 2003-2005  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA. 
- *
- */
-
-/** \file tcas.c
- *  \brief Collision avoidance library
- * 
- */
-
-#include "tcas.h"
-#include "airframe.h"
-#include "estimator.h"
-#include "nav.h"
-#include "gps.h"
-#include "flight_plan.h"
-
-#include "messages.h"
-#include "downlink.h"
-
-float tcas_alt_setpoint;
-float tcas_tau_ta, tcas_tau_ra, tcas_dmod, tcas_alim;
-
-uint8_t tcas_status;
-enum tcas_resolve tcas_resolve;
-uint8_t tcas_ac_RA;
-struct tcas_ac_status tcas_acs_status[NB_ACS];
-
-#ifndef TCAS_TAU_TA     // Traffic Advisory
-#define TCAS_TAU_TA 2*CARROT
-#endif
-
-#ifndef TCAS_TAU_RA     // Resolution Advisory
-#define TCAS_TAU_RA CARROT
-#endif
-
-#ifndef TCAS_DMOD       // Distance Modification
-#define TCAS_DMOD 10.
-#endif
-
-#ifndef TCAS_ALIM       // Altitude Limit
-#define TCAS_ALIM 15.
-#endif
-
-#ifndef TCAS_DT_MAX     // ms (lost com and timeout)
-#define TCAS_DT_MAX 1500
-#endif
-
-#define TCAS_HUGE_TAU 100*TCAS_TAU_TA
-
-/* AC is inside the horizontol dmod area and twice the vertical alim 
separation */
-#define TCAS_IsInside() ( (ddh < Square(tcas_dmod) && ddv < 
Square(2*tcas_alim)) ? 1 : 0 )
-
-void tcas_init( void ) {
-  tcas_alt_setpoint = GROUND_ALT + SECURITY_HEIGHT;
-  tcas_tau_ta = TCAS_TAU_TA;
-  tcas_tau_ra = TCAS_TAU_RA;
-  tcas_dmod = TCAS_DMOD;
-  tcas_alim = TCAS_ALIM;
-  tcas_status = TCAS_NO_ALARM;
-  tcas_resolve = RA_NONE;
-  tcas_ac_RA = AC_ID;
-  uint8_t i;
-  for (i = 0; i < NB_ACS; i++) {
-    tcas_acs_status[i].status = TCAS_NO_ALARM;
-    tcas_acs_status[i].resolve = RA_NONE;
-  }
-}
-
-static inline enum tcas_resolve tcas_test_direction(uint8_t id) {
-  struct ac_info_ * ac = get_ac_info(id);
-  float dz = ac->alt - estimator_z;
-  if (dz > tcas_alim) return RA_DESCEND;
-  else if (dz < -tcas_alim) return RA_CLIMB;
-  else // AC with the smallest ID descend
-  {
-    if (AC_ID < id) return RA_DESCEND;
-    else return RA_CLIMB;
-  }
-}
-
-
-/* conflicts detection and monitoring */
-void tcas_periodic_task_1Hz( void ) {
-  // no TCAS under security_height
-  if (estimator_z < GROUND_ALT + SECURITY_HEIGHT) {
-    uint8_t i;
-    for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM;
-    return;
-  }
-  // test possible conflicts
-  float tau_min = tcas_tau_ta;
-  uint8_t ac_id_close = AC_ID;
-  uint8_t i;
-  float vx = estimator_hspeed_mod * sinf(estimator_hspeed_dir);
-  float vy = estimator_hspeed_mod * cosf(estimator_hspeed_dir);
-  for (i = 2; i < NB_ACS; i++) {
-    if (the_acs[i].ac_id == 0) continue; // no AC data
-    uint32_t dt = gps_itow - the_acs[i].itow;
-    if (dt > 3*TCAS_DT_MAX) {
-      tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status
-      continue;
-    }
-    if (dt > TCAS_DT_MAX) continue; // lost com but keep current status
-    float dx = the_acs[i].east - estimator_x;
-    float dy = the_acs[i].north - estimator_y;
-    float dz = the_acs[i].alt - estimator_z;
-    float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course);
-    float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course);
-    float dvz = estimator_z_dot - the_acs[i].climb;
-    float scal = dvx*dx + dvy*dy + dvz*dz;
-    float ddh = dx*dx + dy*dy;
-    float ddv = dz*dz;
-    float tau = TCAS_HUGE_TAU;
-    if (scal > 0.) tau = (ddh + ddv) / scal;
-    // monitor conflicts
-    uint8_t inside = TCAS_IsInside();
-    //enum tcas_resolve test_dir = RA_NONE;
-    switch (tcas_acs_status[i].status) {
-      case TCAS_RA:
-        if (tau >= TCAS_HUGE_TAU && !inside) {
-          tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now 
resolved
-          tcas_acs_status[i].resolve = RA_NONE;
-          DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel,&(the_acs[i].ac_id));
-        }
-        break;
-      case TCAS_TA:
-        if (tau < tcas_tau_ra || inside) {
-          tcas_acs_status[i].status = TCAS_RA; // TA -> RA
-          // Downlink alert
-          //test_dir = tcas_test_direction(the_acs[i].ac_id);
-          
//DOWNLINK_SEND_TCAS_RA(DefaultChannel,&(the_acs[i].ac_id),&test_dir);// FIXME 
only one closest AC ???
-          break;
-        }
-        if (tau > tcas_tau_ta && !inside)
-          tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now 
resolved
-          tcas_acs_status[i].resolve = RA_NONE;
-          DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel,&(the_acs[i].ac_id));
-        break;
-      case TCAS_NO_ALARM:
-        if (tau < tcas_tau_ta || inside) {
-          tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA
-          // Downlink warning
-          DOWNLINK_SEND_TCAS_TA(DefaultChannel,&(the_acs[i].ac_id));
-        }
-        if (tau < tcas_tau_ra || inside) {
-          tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem 
?
-          // Downlink alert
-          //test_dir = tcas_test_direction(the_acs[i].ac_id);
-          
//DOWNLINK_SEND_TCAS_RA(DefaultChannel,&(the_acs[i].ac_id),&test_dir);
-        }
-        break;
-    }
-    // store closest AC
-    if (tau < tau_min) {
-      tau_min = tau;
-      ac_id_close = the_acs[i].ac_id;
-
-    }
-  }
-  // set current conflict mode
-  if (tcas_status == TCAS_RA && tcas_ac_RA != AC_ID && 
tcas_acs_status[the_acs_id[tcas_ac_RA]].status == TCAS_RA) {
-    ac_id_close = tcas_ac_RA; // keep RA until resolved
-  }
-  tcas_status = tcas_acs_status[the_acs_id[ac_id_close]].status;
-  // at least one in conflict, deal with closest one
-  if (tcas_status == TCAS_RA) {
-    tcas_ac_RA = ac_id_close;
-    tcas_resolve = tcas_test_direction(tcas_ac_RA);
-    uint8_t ac_resolve = tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve;
-    if (ac_resolve != RA_NONE) { // first resolution, no message received
-      if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
-        if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
-        else tcas_resolve = RA_CLIMB;
-      }
-      tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve = RA_LEVEL; // assuming 
level flight for now
-    }
-    else { // second resolution or message received
-      if (ac_resolve != RA_LEVEL) { // message received
-        if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
-          if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
-          else tcas_resolve = RA_CLIMB;
-        }
-      }
-      else { // no message
-        if (tcas_resolve == RA_CLIMB && the_acs[the_acs_id[tcas_ac_RA]].climb 
> 1.0) tcas_resolve = RA_DESCEND; // revert resolve
-        else if (tcas_resolve == RA_DESCEND && 
the_acs[the_acs_id[tcas_ac_RA]].climb < -1.0) tcas_resolve = RA_CLIMB; // 
revert resolve
-      }
-    }
-    // Downlink alert
-    DOWNLINK_SEND_TCAS_RA(DefaultChannel,&tcas_ac_RA,&tcas_resolve);
-  }
-  else tcas_ac_RA = AC_ID; // no conflict
-#ifdef TCAS_DEBUG
-  if (tcas_status == TCAS_RA) 
DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel,&ac_id_close,&tau_min);
-#endif
-}
-
-
-/* altitude control loop */
-void tcas_periodic_task_4Hz( void ) {
-  // set alt setpoint
-  if (estimator_z > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) {
-    struct ac_info_ * ac = get_ac_info(tcas_ac_RA);
-    switch (tcas_resolve) {
-      case RA_CLIMB :
-        tcas_alt_setpoint = Max(nav_altitude, ac->alt + tcas_alim);
-        break;
-      case RA_DESCEND :
-        tcas_alt_setpoint = Min(nav_altitude, ac->alt - tcas_alim);
-        break;
-      case RA_LEVEL :
-      case RA_NONE :
-        tcas_alt_setpoint = nav_altitude;
-        break;
-    }
-    // Bound alt
-    tcas_alt_setpoint = Max(GROUND_ALT + SECURITY_HEIGHT, tcas_alt_setpoint);
-  }
-  else {
-    tcas_alt_setpoint = nav_altitude;
-    tcas_resolve = RA_NONE;
-  }
-}

Deleted: paparazzi3/trunk/sw/airborne/tcas.h
===================================================================
--- paparazzi3/trunk/sw/airborne/tcas.h 2010-10-25 08:54:38 UTC (rev 6225)
+++ paparazzi3/trunk/sw/airborne/tcas.h 2010-10-25 08:58:08 UTC (rev 6226)
@@ -1,61 +0,0 @@
-/*
- * Paparazzi mcu0 $Id$
- *  
- * Copyright (C) 2003-2005  Pascal Brisset, Antoine Drouin
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA. 
- *
- */
-
-/** \file tcas.h
- *  \brief Collision avoidance library
- * 
- */
-
-#ifndef TCAS_H
-#define TCAS_H
-
-#include "std.h"
-#include "traffic_info.h"
-
-extern float tcas_alt_setpoint;
-extern float tcas_tau_ta, tcas_tau_ra, tcas_dmod, tcas_alim;
-
-#define TCAS_NO_ALARM 0
-#define TCAS_TA 1
-#define TCAS_RA 2
-enum tcas_resolve { RA_NONE, RA_LEVEL, RA_CLIMB, RA_DESCEND };
-
-extern uint8_t tcas_status;
-extern enum tcas_resolve tcas_resolve;
-extern uint8_t tcas_ac_RA;
-
-struct tcas_ac_status {
-  uint8_t status;
-  enum tcas_resolve resolve;
-};
-
-extern struct tcas_ac_status tcas_acs_status[NB_ACS];
-
-extern void tcas_init( void );
-extern void tcas_periodic_task_1Hz( void );
-extern void tcas_periodic_task_4Hz( void );
-
-#define CallTCAS() { if (tcas_status == TCAS_RA) v_ctl_altitude_setpoint = 
tcas_alt_setpoint; }
-
-#endif // TCAS