traffic info not limited by AC id;

formation flight functions improved but need pre_call for leader

sw/airborne/datalink.c

@@ -69,8 +69,9 @@ void dl_parse_msg(void) {
     float a = MOfCm(DL_ACINFO_alt(dl_buffer));
     float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
     float s = MOfCm(DL_ACINFO_speed(dl_buffer));
+    float cl = MOfCm(DL_ACINFO_climb(dl_buffer));
     uint32_t t = DL_ACINFO_itow(dl_buffer);
-    SetAcInfo(id, ux, uy, c, a, s, t);
+    SetAcInfo(id, ux, uy, c, a, s, cl, t);
   } else
 #endif
 #ifdef NAV
@@ -145,8 +146,8 @@ void dl_parse_msg(void) {
     uint8_t leader = DL_FORMATION_STATUS_leader_id(dl_buffer);
     uint8_t status = DL_FORMATION_STATUS_status(dl_buffer);
     if (ac_id == AC_ID) leader_id = leader;
-    else if (leader == leader_id) formation[ac_id].status = status;
-    else formation[ac_id].status = UNSET;
+    else if (leader == leader_id) { UpdateFormationStatus(ac_id,status); }
+    else { UpdateFormationStatus(ac_id,UNSET); }
   } else
 #endif
   { /* Last else */ }

sw/airborne/formation.c

@@ -36,6 +36,8 @@ int form_mode;
 uint8_t leader_id;
 float old_cruise, old_alt;
 
+struct slot_ formation[NB_ACS];
+
 #ifndef FORM_CARROT
 #define FORM_CARROT 2.
 #endif
@@ -82,11 +84,12 @@ int formation_init(void) {
 }
 
 int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a) {
+  if (_id != AC_ID && the_acs_id[_id] == 0) return FALSE; // no info for this AC
   DOWNLINK_SEND_FORMATION_SLOT_TM(&_id, &form_mode, &slot_e, &slot_n, &slot_a);
-  formation[_id].status = IDLE;
-  formation[_id].east = slot_e;
-  formation[_id].north = slot_n;
-  formation[_id].alt = slot_a;
+  formation[the_acs_id[_id]].status = IDLE;
+  formation[the_acs_id[_id]].east = slot_e;
+  formation[the_acs_id[_id]].north = slot_n;
+  formation[the_acs_id[_id]].alt = slot_a;
   return FALSE;
 }
 
@@ -96,7 +99,8 @@ int start_formation(void) {
   for (i = 0; i < NB_ACS; ++i) {
     if (formation[i].status == IDLE) formation[i].status = ACTIVE;
   }
-  DOWNLINK_SEND_FORMATION_STATUS_TM(&ac_id,&leader_id,&formation[AC_ID].status);
+  enum slot_status active = ACTIVE;
+  DOWNLINK_SEND_FORMATION_STATUS_TM(&ac_id,&leader_id,&active);
   // store current cruise and alt
   old_cruise = v_ctl_auto_throttle_cruise_throttle;
   old_alt = nav_altitude;
@@ -109,7 +113,8 @@ int stop_formation(void) {
   for (i = 0; i < NB_ACS; ++i) {
     if (formation[i].status == ACTIVE) formation[i].status = IDLE;
   }
-  DOWNLINK_SEND_FORMATION_STATUS_TM(&ac_id,&leader_id,&formation[AC_ID].status);
+  enum slot_status idle = IDLE;
+  DOWNLINK_SEND_FORMATION_STATUS_TM(&ac_id,&leader_id,&idle);
   // restore cruise and alt
   v_ctl_auto_throttle_cruise_throttle = old_cruise;
   old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
@@ -132,29 +137,29 @@ int formation_flight(void) {
   form_speed_n = estimator_hspeed_mod * ch;
   form_speed_e = estimator_hspeed_mod * sh;
 
+  if (AC_ID == leader_id) {
+    estimator_x += formation[the_acs_id[AC_ID]].east;
+    estimator_y += formation[the_acs_id[AC_ID]].north;
+  }
+  // set info for this AC
+  SetAcInfo(AC_ID, estimator_x, estimator_y, estimator_hspeed_dir, estimator_z, estimator_hspeed_mod, estimator_z_dot, gps_itow);
+
   // broadcast info
   uint8_t ac_id = AC_ID;
-  DOWNLINK_SEND_FORMATION_STATUS_TM(&ac_id,&leader_id,&formation[AC_ID].status);
+  enum slot_status status = formation[the_acs_id[AC_ID]].status;
+  DOWNLINK_SEND_FORMATION_STATUS_TM(&ac_id,&leader_id,&status);
   if (++_1Hz>=4) {
     _1Hz=0;
     DOWNLINK_SEND_FORMATION_SLOT_TM(&ac_id, &form_mode,
-        &formation[AC_ID].east,
-        &formation[AC_ID].north,
-        &formation[AC_ID].alt);
+        &formation[the_acs_id[AC_ID]].east,
+        &formation[the_acs_id[AC_ID]].north,
+        &formation[the_acs_id[AC_ID]].alt);
   }
-
-  // set info for this AC
-  SetAcInfo(AC_ID, estimator_x, estimator_y, estimator_hspeed_dir, estimator_z, estimator_hspeed_mod, gps_itow);
-  if (formation[AC_ID].status != ACTIVE) return FALSE; // AC not ready
+  if (formation[the_acs_id[AC_ID]].status != ACTIVE) return FALSE; // AC not ready
 
   // get leader info
   struct ac_info_ * leader = get_ac_info(leader_id);
-  if (formation[leader_id].status != ACTIVE) return FALSE; // leader not ready
-  //if (formation[leader_id].status == UNSET) return FALSE; // leader not ready
-  //else if (formation[leader_id].status == IDLE) {
-  //    if(Max(((int)gps_itow - (int)leader->itow) / 1000., 0.) > FORM_CARROT) return TRUE; // still not ready
-  //    else formation[leader_id].status = ACTIVE;
-  //}
+  if (formation[the_acs_id[leader_id]].status != ACTIVE) return FALSE; // leader not ready
 
   // compute slots in the right reference frame
   struct slot_ form[NB_ACS];
@@ -172,11 +177,10 @@ int formation_flight(void) {
 
   // compute control forces
   for (i = 0; i < NB_ACS; ++i) {
-    if (formation[i].status == UNSET || i == AC_ID) continue;
-    struct ac_info_ * ac = get_ac_info(i);
+    if (formation[i].status != ACTIVE || the_acs[i].ac_id == AC_ID) continue;
+    struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
     if (fabs(estimator_z - ac->alt) < form_prox && ac->alt > 0) {
-      float delta_t = Max(((int)gps_itow - (int)ac->itow) / 1000., 0.);
-      //printf("dt %d %d %u %u %f \n",AC_ID,i,gps_itow,ac->itow,delta_t);
+      float delta_t = Max((int)(gps_itow - ac->itow) / 1000., 0.);
       if (delta_t > FORM_CARROT) {
         // if AC not responding for too long
         formation[i].status = IDLE;
@@ -184,10 +188,10 @@ int formation_flight(void) {
       }
       else formation[i].status = ACTIVE;
       form_e += (ac->east  + ac->gspeed*sin(ac->course)*delta_t - estimator_x)
-        - (form[i].east - form[AC_ID].east);
+        - (form[i].east - form[the_acs_id[AC_ID]].east);
       form_n += (ac->north + ac->gspeed*cos(ac->course)*delta_t - estimator_y)
-        - (form[i].north - form[AC_ID].north);
-      form_a += (ac->alt - estimator_z) - (formation[i].alt - formation[AC_ID].alt);
+        - (form[i].north - form[the_acs_id[AC_ID]].north);
+      form_a += (ac->alt - estimator_z) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
       form_speed += ac->gspeed;
       //form_speed_e += ac->gspeed * sin(ac->course);
       //form_speed_n += ac->gspeed * cos(ac->course);
@@ -209,24 +213,20 @@ int formation_flight(void) {
   // altitude loop
   float alt = 0.;
   if (AC_ID == leader_id) alt = nav_altitude;
-  else alt = leader->alt - form[leader_id].alt;
-  alt += formation[AC_ID].alt + coef_form_alt * form_a;
-  //NavVerticalAltitudeMode(Max(alt, ground_alt+SECURITY_HEIGHT), 0.);
+  else alt = leader->alt - form[the_acs_id[leader_id]].alt;
+  alt += formation[the_acs_id[AC_ID]].alt + coef_form_alt * form_a;
   flight_altitude = Max(alt, ground_alt+SECURITY_HEIGHT);
 
   // carrot
   if (AC_ID != leader_id) {
-    float dx = form[AC_ID].east - form[leader_id].east;
-    float dy = form[AC_ID].north - form[leader_id].north;
+    float dx = form[the_acs_id[AC_ID]].east - form[the_acs_id[leader_id]].east;
+    float dy = form[the_acs_id[AC_ID]].north - form[the_acs_id[leader_id]].north;
     desired_x = leader->east  + NOMINAL_AIRSPEED * form_carrot * sin(leader->course) + dx;
     desired_y = leader->north + NOMINAL_AIRSPEED * form_carrot * cos(leader->course) + dy;
-    // scale course_pgain only for followers
-    h_ctl_course_pgain = -coef_form_course;
     // fly to desired
     fly_to_xy(desired_x, desired_y);
     desired_x = leader->east  + dx;
     desired_y = leader->north + dy;
-    //fly_to_xy(desired_x, desired_y);
     // lateral correction
     //float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
     //float diff_course = leader->course - estimator_hspeed_dir;
@@ -234,27 +234,22 @@ int formation_flight(void) {
     //h_ctl_roll_setpoint += coef_form_course * diff_course;
     //h_ctl_roll_setpoint += coef_form_course * diff_heading;
   }
-  else {
-    // virtual leader at the center of the formation
-    estimator_x -= form[leader_id].east;
-    estimator_y -= form[leader_id].north;
-    // fly to desired
-    fly_to_xy(desired_x, desired_y);
-    // restore real position
-    estimator_x += form[leader_id].east;
-    estimator_y += form[leader_id].north;
-    //desired_x += form[leader_id].east;
-    //desired_y += form[leader_id].north;
-  }
   //BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
 
   // speed loop
-  float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
-  cruise += coef_form_pos * (form_n * ch + form_e * sh) + coef_form_speed * form_speed;
-  Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
-  v_ctl_auto_throttle_cruise_throttle = cruise;
-
+  if (nb > 0) {
+    float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
+    cruise += coef_form_pos * (form_n * ch + form_e * sh) + coef_form_speed * form_speed;
+    Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
+    v_ctl_auto_throttle_cruise_throttle = cruise;
+  }
   return TRUE;
 }
 
+void formation_pre_call(void) {
+  if (leader_id == AC_ID) {
+    estimator_x -= formation[the_acs_id[AC_ID]].east;
+    estimator_y -= formation[the_acs_id[AC_ID]].north;
+  }
+}
 

sw/airborne/formation.h

@@ -27,28 +27,26 @@ struct slot_ {
     float alt;
 };
 
-struct slot_ formation[NB_ACS];
+extern struct slot_ formation[NB_ACS];
 
-int formation_init(void);
+extern int formation_init(void);
 
-int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a);
+extern int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a);
 
 #define UpdateSlot(_id, _se, _sn, _sa) { \
-  formation[_id].east = _se; \
-  formation[_id].north = _sn; \
-  formation[_id].alt = _sa; \
+  formation[the_acs_id[_id]].east = _se; \
+  formation[the_acs_id[_id]].north = _sn; \
+  formation[the_acs_id[_id]].alt = _sa; \
 }
 
-int start_formation(void);
+#define UpdateFormationStatus(_id,_status) { formation[the_acs_id[_id]].status = _status; }
 
-int stop_formation(void);
+extern int start_formation(void);
 
-int formation_flight(void);
+extern int stop_formation(void);
 
-#ifdef FORMATION 
-#define FormationFlight() formation_flight()
-#else
-#define FormationFlight() {}
-#endif
+extern int formation_flight(void);
+
+extern void formation_pre_call(void);
 
 #endif /* FORMATION_H */

sw/airborne/main_ap.c

@@ -103,6 +103,10 @@
 #include "formation.h"
 #endif
 
+#ifdef TCAS
+#include "tcas.h"
+#endif
+
 #if ! defined CATASTROPHIC_BAT_LEVEL && defined LOW_BATTERY
 #warning "LOW_BATTERY deprecated. Renammed into CATASTROPHIC_BAT_LEVEL (in aiframe file)"
 #define CATASTROPHIC_BAT_LEVEL LOW_BATTERY
@@ -313,6 +317,10 @@ static void navigation_task( void ) {
   else
     nav_periodic_task();
   
+#ifdef TCAS
+  CallTCAS();
+#endif
+
 #ifndef PERIOD_NAVIGATION_0 // If not sent periodically (in default 0 mode)
   SEND_NAVIGATION();
 #endif
@@ -417,6 +425,13 @@ void periodic_task_ap( void ) {
     break;
 #endif
 
+#ifdef TCAS
+  case 6:
+    /** conflicts monitoring at 1Hz */
+    tcas_periodic_task_1Hz();
+    break;
+#endif
+
   }
 
 #ifdef USE_LIGHT
@@ -470,6 +485,13 @@ void periodic_task_ap( void ) {
     break;
 #endif
 
+#ifdef TCAS
+  case 14:
+    /** tcas altitude control loop at 4Hz just before navigation task */
+    tcas_periodic_task_4Hz();
+    break;
+#endif
+
     /*  default: */
   }
 
@@ -635,10 +657,18 @@ void init_ap( void ) {
   LightInit();
 #endif
 
+  /************ Multi-uavs status ***************/
+
+  traffic_info_init();
+
 #ifdef FORMATION
   formation_init();
 #endif
 
+#ifdef TCAS
+  tcas_init();
+#endif
+
 }
 
 

sw/airborne/traffic_info.c

@@ -28,10 +28,18 @@
 
 #include <inttypes.h>
 #include "traffic_info.h"
+#include "airframe.h"
 
+uint8_t acs_idx;
+uint8_t the_acs_id[NB_ACS_ID];
 struct ac_info_ the_acs[NB_ACS];
+ 
+void traffic_info_init( void ) {
+  the_acs_id[0] = 0;  // ground station
+  the_acs_id[AC_ID] = 1;
+  acs_idx = 2;
+}
 
 struct ac_info_ * get_ac_info(uint8_t id) {
-  id = id < NB_ACS ? id : 0;
-  return &the_acs[id];
+  return &the_acs[the_acs_id[id]];
 }

sw/airborne/traffic_info.h

@@ -29,31 +29,44 @@
 #ifndef TI_H
 #define TI_H
 
+#define NB_ACS_ID 256
 #define NB_ACS 24
 
 struct ac_info_ {
+  uint8_t ac_id;
   float east; /* m */
   float north; /* m */
   float course; /* rad (CW) */
   float alt; /* m */
   float gspeed; /* m/s */
+  float climb; /* m/s */
   uint32_t itow; /* ms */
 };
 
+extern uint8_t acs_idx;
+extern uint8_t the_acs_id[NB_ACS_ID];
 extern struct ac_info_ the_acs[NB_ACS];
 
-#define SetAcInfo(_id, _utm_x /*m*/, _utm_y /*m*/, _course/*rad(CW)*/, _alt/*m*/,_gspeed/*m/s*/, _itow) { \
-  if (_id < NB_ACS) { \
-    the_acs[_id].east = _utm_x -  nav_utm_east0; \
-    the_acs[_id].north = _utm_y - nav_utm_north0; \
-    the_acs[_id].course = _course; \
-    the_acs[_id].alt = _alt; \
-    the_acs[_id].gspeed = _gspeed; \
-    the_acs[_id].itow = (uint32_t)_itow; \
+// 0 is reserved for ground station (id=0)
+// 1 is reserved for this AC (id=AC_ID)
+#define SetAcInfo(_id, _utm_x /*m*/, _utm_y /*m*/, _course/*rad(CW)*/, _alt/*m*/,_gspeed/*m/s*/,_climb, _itow) { \
+  if (acs_idx < NB_ACS) { \
+    if (_id > 0 && the_acs_id[_id] == 0) { \
+      the_acs_id[_id] = acs_idx++; \
+      the_acs[the_acs_id[_id]].ac_id = (uint8_t)_id; \
+    } \
+    the_acs[the_acs_id[_id]].east = _utm_x -  nav_utm_east0; \
+    the_acs[the_acs_id[_id]].north = _utm_y - nav_utm_north0; \
+    the_acs[the_acs_id[_id]].course = _course; \
+    the_acs[the_acs_id[_id]].alt = _alt; \
+    the_acs[the_acs_id[_id]].gspeed = _gspeed; \
+    the_acs[the_acs_id[_id]].climb = _climb; \
+    the_acs[the_acs_id[_id]].itow = (uint32_t)_itow; \
   } \
 }
 
-struct ac_info_ *
-get_ac_info(uint8_t id);
+extern void traffic_info_init( void );
+
+struct ac_info_ * get_ac_info(uint8_t id);
 
 #endif