int formation_flight(void)
{
static uint8_t _1Hz = 0;
uint8_t nb = 0, i;
float hspeed_dir = stateGetHorizontalSpeedDir_f();
float ch = cosf(hspeed_dir);
float sh = sinf(hspeed_dir);
form_n = 0.;
form_e = 0.;
form_a = 0.;
form_speed = stateGetHorizontalSpeedNorm_f();
form_speed_n = form_speed * ch;
form_speed_e = form_speed * sh;
if (AC_ID == leader_id) {
stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east;
stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north;
}
// set info for this AC
set_ac_info(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, hspeed_dir,
stateGetPositionUtm_f()->alt, form_speed, stateGetSpeedEnu_f()->z, gps.tow);
// broadcast info
uint8_t ac_id = AC_ID;
uint8_t status = formation[the_acs_id[AC_ID]].status;
DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &status);
if (++_1Hz >= 4) {
_1Hz = 0;
DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &ac_id, &form_mode,
&formation[the_acs_id[AC_ID]].east,
&formation[the_acs_id[AC_ID]].north,
&formation[the_acs_id[AC_ID]].alt);
}
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[the_acs_id[leader_id]].status == UNSET ||
formation[the_acs_id[leader_id]].status == IDLE) {
// leader not ready or not in formation
return FALSE;
}
// compute slots in the right reference frame
struct slot_ form[NB_ACS];
float cr = 0., sr = 1.;
if (form_mode == FORM_MODE_COURSE) {
cr = cosf(leader->course);
sr = sinf(leader->course);
}
for (i = 0; i < NB_ACS; ++i) {
if (formation[i].status == UNSET) { continue; }
form[i].east = formation[i].east * sr - formation[i].north * cr;
form[i].north = formation[i].east * cr + formation[i].north * sr;
form[i].alt = formation[i].alt;
}
// compute control forces
for (i = 0; i < NB_ACS; ++i) {
if (the_acs[i].ac_id == AC_ID) { continue; }
struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id);
float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.);
if (delta_t > FORM_CARROT) {
// if AC not responding for too long
formation[i].status = LOST;
continue;
} else {
// compute control if AC is ACTIVE and around the same altitude (maybe not so usefull)
formation[i].status = ACTIVE;
if (ac->alt > 0 && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox) {
form_e += (ac->east + ac->gspeed * sinf(ac->course) * delta_t - stateGetPositionEnu_f()->x)
- (form[i].east - form[the_acs_id[AC_ID]].east);
form_n += (ac->north + ac->gspeed * cosf(ac->course) * delta_t - stateGetPositionEnu_f()->y)
- (form[i].north - form[the_acs_id[AC_ID]].north);
form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt);
form_speed += ac->gspeed;
//form_speed_e += ac->gspeed * sinf(ac->course);
//form_speed_n += ac->gspeed * cosf(ac->course);
++nb;
}
}
}
uint8_t _nb = Max(1, nb);
form_n /= _nb;
form_e /= _nb;
form_a /= _nb;
form_speed = form_speed / (nb + 1) - stateGetHorizontalSpeedNorm_f();
//form_speed_e = form_speed_e / (nb+1) - stateGetHorizontalSpeedNorm_f() * sh;
//form_speed_n = form_speed_n / (nb+1) - stateGetHorizontalSpeedNorm_f() * ch;
// set commands
NavVerticalAutoThrottleMode(0.);
// altitude loop
float alt = 0.;
if (AC_ID == leader_id) {
alt = nav_altitude;
} 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[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 * sinf(leader->course) + dx;
desired_y = leader->north + NOMINAL_AIRSPEED * form_carrot * cosf(leader->course) + dy;
// fly to desired
fly_to_xy(desired_x, desired_y);
desired_x = leader->east + dx;
desired_y = leader->north + dy;
// lateral correction
//float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
//float diff_course = leader->course - hspeed_dir;
//NormRadAngle(diff_course);
//h_ctl_roll_setpoint += coef_form_course * diff_course;
//h_ctl_roll_setpoint += coef_form_course * diff_heading;
}
//BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
// speed loop
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;
}
int parse_acinfo_dl()
{
uint8_t sender_id = SenderIdOfPprzMsg(dl_buffer);
uint8_t msg_id = IdOfPprzMsg(dl_buffer);
if (sender_id > 0) {
switch (msg_id) {
case DL_GPS_SMALL: {
uint32_t multiplex_speed = DL_GPS_SMALL_multiplex_speed(dl_buffer);
// decode compressed values
int16_t course = (int16_t)((multiplex_speed >> 21) & 0x7FF); // bits 31-21 course in decideg
if (course & 0x400) {
course |= 0xF800; // fix for twos complements
}
course *= 2; // scale course by resolution
int16_t gspeed = (int16_t)((multiplex_speed >> 10) & 0x7FF); // bits 20-10 ground speed cm/s
if (gspeed & 0x400) {
gspeed |= 0xF800; // fix for twos complements
}
int16_t climb = (int16_t)(multiplex_speed & 0x3FF); // bits 9-0 z climb speed in cm/s
if (climb & 0x200) {
climb |= 0xFC00; // fix for twos complements
}
set_ac_info_lla(sender_id,
DL_GPS_SMALL_lla_lat(dl_buffer),
DL_GPS_SMALL_lla_lon(dl_buffer),
(int32_t)DL_GPS_SMALL_alt(dl_buffer) * 10,
course,
gspeed,
climb,
gps_tow_from_sys_ticks(sys_time.nb_tick));
}
break;
case DL_GPS: {
set_ac_info(sender_id,
DL_GPS_utm_east(dl_buffer),
DL_GPS_utm_north(dl_buffer),
DL_GPS_alt(dl_buffer),
DL_GPS_utm_zone(dl_buffer),
DL_GPS_course(dl_buffer),
DL_GPS_speed(dl_buffer),
DL_GPS_climb(dl_buffer),
DL_GPS_itow(dl_buffer));
}
break;
case DL_GPS_LLA: {
set_ac_info_lla(sender_id,
DL_GPS_LLA_lat(dl_buffer),
DL_GPS_LLA_lon(dl_buffer),
DL_GPS_LLA_alt(dl_buffer),
DL_GPS_LLA_course(dl_buffer),
DL_GPS_LLA_speed(dl_buffer),
DL_GPS_LLA_climb(dl_buffer),
DL_GPS_LLA_itow(dl_buffer));
}
break;
default:
return 0;
}
} else {
switch (msg_id) {
