diff --git a/sw/airborne/datalink.c b/sw/airborne/datalink.c index 518d056df5..2268ff0fcc 100644 --- a/sw/airborne/datalink.c +++ b/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 */ } diff --git a/sw/airborne/formation.c b/sw/airborne/formation.c index f0b639e304..1881d95bad 100644 --- a/sw/airborne/formation.c +++ b/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; + } +} diff --git a/sw/airborne/formation.h b/sw/airborne/formation.h index 40ce6c8f20..0cbfdf47b1 100644 --- a/sw/airborne/formation.h +++ b/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 */ diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c index 3126c3e284..8219101bad 100644 --- a/sw/airborne/main_ap.c +++ b/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 + } diff --git a/sw/airborne/traffic_info.c b/sw/airborne/traffic_info.c index c24acf47b1..e9b5be55b8 100644 --- a/sw/airborne/traffic_info.c +++ b/sw/airborne/traffic_info.c @@ -28,10 +28,18 @@ #include #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]]; } diff --git a/sw/airborne/traffic_info.h b/sw/airborne/traffic_info.h index 29c2da2f13..dd3aaecee0 100644 --- a/sw/airborne/traffic_info.h +++ b/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