[paparazzi-commits] [5608] tcas are talking to each other to avoid conflicting resolutions

[Gautier Hattenberger]

Revision: 5608
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5608
Author:   gautier
Date:     2010-08-23 13:35:08 +0000 (Mon, 23 Aug 2010)
Log Message:
-----------
tcas are talking to each other to avoid conflicting resolutions

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/datalink.c
    paparazzi3/trunk/sw/airborne/tcas.c
    paparazzi3/trunk/sw/airborne/tcas.h
    paparazzi3/trunk/sw/ground_segment/tmtc/fw_server.ml

Modified: paparazzi3/trunk/sw/airborne/datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/datalink.c     2010-08-23 13:24:02 UTC (rev 
5607)
+++ paparazzi3/trunk/sw/airborne/datalink.c     2010-08-23 13:35:08 UTC (rev 
5608)
@@ -41,6 +41,10 @@
 #include "traffic_info.h"
 #endif // TRAFFIC_INFO
 
+#ifdef TCAS
+#include "tcas.h"
+#endif
+
 #ifdef USE_JOYSTICK
 #include "joystick.h"
 #endif
@@ -78,7 +82,26 @@
 void dl_parse_msg(void) {
   datalink_time = 0;
   uint8_t msg_id = IdOfMsg(dl_buffer);
+#if 0 // not ready yet
+  uint8_t sender_id = SenderIdOfMsg(dl_buffer);
 
+  /* parse telemetry messages coming from other AC */
+  if (sender_id != 0) {
+    switch (msg_id) {
+#ifdef TCAS
+      case DL_TCAS_RA:
+        {
+          if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && 
SenderIdOfMsg(dl_buffer) != AC_ID) {
+            uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer);
+            tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = 
DL_TCAS_RA_resolve(dl_buffer);
+          }
+        }
+#endif
+    }
+    return;
+  }
+#endif
+
   if (msg_id == DL_PING) {
     DOWNLINK_SEND_PONG(DefaultChannel);
   } else
@@ -116,8 +139,14 @@
     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) {
+  if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
     wind_east = DL_WIND_INFO_east(dl_buffer);
     wind_north = DL_WIND_INFO_north(dl_buffer);
 #ifndef USE_AIRSPEED

Modified: paparazzi3/trunk/sw/airborne/tcas.c
===================================================================
--- paparazzi3/trunk/sw/airborne/tcas.c 2010-08-23 13:24:02 UTC (rev 5607)
+++ paparazzi3/trunk/sw/airborne/tcas.c 2010-08-23 13:35:08 UTC (rev 5608)
@@ -41,8 +41,9 @@
 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;
-uint8_t tcas_acs_status[NB_ACS];
+struct tcas_ac_status tcas_acs_status[NB_ACS];
 
 #ifndef TCAS_TAU_TA     // Traffic Advisory
 #define TCAS_TAU_TA 2*CARROT
@@ -76,18 +77,34 @@
   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] = TCAS_NO_ALARM;
+  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] = TCAS_NO_ALARM;
+    for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM;
     return;
   }
   // test possible conflicts
@@ -100,7 +117,7 @@
     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] = TCAS_NO_ALARM; // timeout, reset status
+      tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status
       continue;
     }
     if (dt > TCAS_DT_MAX) continue; // lost com but keep current status
@@ -117,31 +134,39 @@
     if (scal > 0.) tau = (ddh + ddv) / scal;
     // monitor conflicts
     uint8_t inside = TCAS_IsInside();
-    switch (tcas_acs_status[i]) {
+    //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] = TCAS_NO_ALARM; // conflict is now resolved
+        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] = TCAS_RA; // TA -> RA
+          tcas_acs_status[i].status = TCAS_RA; // TA -> RA
           // Downlink alert
-          DOWNLINK_SEND_TCAS_RA(DefaultChannel,&(the_acs[i].ac_id));
+          //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] = TCAS_NO_ALARM; // conflict is now resolved
+          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] = TCAS_TA; // NO_ALARM -> TA
+          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] = TCAS_RA; // NO_ALARM -> RA = big problem ?
+          tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem 
?
           // Downlink alert
-          DOWNLINK_SEND_TCAS_RA(DefaultChannel,&(the_acs[i].ac_id));
+          //test_dir = tcas_test_direction(the_acs[i].ac_id);
+          
//DOWNLINK_SEND_TCAS_RA(DefaultChannel,&(the_acs[i].ac_id),&test_dir);
         }
         break;
     }
@@ -149,14 +174,42 @@
     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]] == TCAS_RA)) {
-    tcas_status = tcas_acs_status[the_acs_id[ac_id_close]];
-    if (tcas_status == TCAS_RA) tcas_ac_RA = ac_id_close;
-    else tcas_ac_RA = AC_ID; // no conflicts
+  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
@@ -166,20 +219,25 @@
 /* altitude control loop */
 void tcas_periodic_task_4Hz( void ) {
   // set alt setpoint
-  if (estimator_z > GROUND_ALT + SECURITY_HEIGHT) {
+  if (estimator_z > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) {
     struct ac_info_ * ac = get_ac_info(tcas_ac_RA);
-    float dz = ac->alt - estimator_z;
-    if (dz > tcas_alim) // go down
-      tcas_alt_setpoint = Min(nav_altitude, ac->alt - tcas_alim);
-    if (dz < -tcas_alim) // go up
-      tcas_alt_setpoint = Max(nav_altitude, ac->alt + tcas_alim);
-    else // AC with the smallest ID goes down
-    {
-      if (AC_ID < tcas_ac_RA) tcas_alt_setpoint = Min(nav_altitude, ac->alt - 
tcas_alim);
-      else tcas_alt_setpoint = Max(nav_altitude, ac->alt + tcas_alim);
+    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;
+  else {
+    tcas_alt_setpoint = nav_altitude;
+    tcas_resolve = RA_NONE;
+  }
 }

Modified: paparazzi3/trunk/sw/airborne/tcas.h
===================================================================
--- paparazzi3/trunk/sw/airborne/tcas.h 2010-08-23 13:24:02 UTC (rev 5607)
+++ paparazzi3/trunk/sw/airborne/tcas.h 2010-08-23 13:35:08 UTC (rev 5608)
@@ -39,11 +39,19 @@
 #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;
 
-extern uint8_t tcas_acs_status[NB_ACS];
+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 );

Modified: paparazzi3/trunk/sw/ground_segment/tmtc/fw_server.ml
===================================================================
--- paparazzi3/trunk/sw/ground_segment/tmtc/fw_server.ml        2010-08-23 
13:24:02 UTC (rev 5607)
+++ paparazzi3/trunk/sw/ground_segment/tmtc/fw_server.ml        2010-08-23 
13:35:08 UTC (rev 5608)
@@ -279,5 +279,12 @@
       Dl_Pprz.message_send "ground_dl" "FORMATION_SLOT" values
   | "FORMATION_STATUS_TM" ->
       Dl_Pprz.message_send "ground_dl" "FORMATION_STATUS" values
+  | "TCAS_RA" ->
+      let vs = [
+        "ac_id", Pprz.Int (ivalue "ac_id");
+        "ac_id_conflict", Pprz.Int (int_of_string a.id);
+        "resolve", Pprz.Int (ivalue "resolve")
+      ] in
+      Dl_Pprz.message_send "ground_dl" "TCAS_RESOLVE" vs
   | _ -> ()