Esempio n. 1
0
void cam_ac_target( void ) {
#ifdef TRAFFIC_INFO
  struct ac_info_ * ac = get_ac_info(cam_target_ac);
  cam_target_x = ac->east;
  cam_target_y = ac->north;
  cam_target_alt = ac->alt;
  cam_target();
#endif // TRAFFIC_INFO
}
Esempio n. 2
0
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/2) return RA_DESCEND;
  else if (dz < -tcas_alim/2) return RA_CLIMB;
  else // AC with the smallest ID descend
  {
    if (AC_ID < id) return RA_DESCEND;
    else return RA_CLIMB;
  }
}
Esempio n. 3
0
static inline enum tcas_resolve tcas_test_direction(uint8_t id)
{
  struct ac_info_ * ac = get_ac_info(id);
  float dz = ac->alt - stateGetPositionUtm_f()->alt;
  if (dz > tcas_alim / 2) { return RA_DESCEND; }
  else if (dz < -tcas_alim / 2) { return RA_CLIMB; }
  else { // AC with the smallest ID descend
    if (AC_ID < id) { return RA_DESCEND; }
    else { return RA_CLIMB; }
  }
}
Esempio n. 4
0
void ant_tracker_periodic( void ) {
  if (ant_track_mode == ANT_TRACK_AUTO) {
    struct ac_info_ * ac = get_ac_info(ant_track_id);
    ant_track_azim =  atan2(ac->north, ac->east) * 180. / M_PI;
    ant_track_azim = 90. - ant_track_azim;
    if (ant_track_azim < 0)
      ant_track_azim += 360.;
    float dist = sqrt(ac->north*ac->north + ac->east*ac->east);
    if ( dist < 1.) dist = 1.;
    float height = ac->alt - ant_track_elev;
    ant_track_elev =  atan2( height, dist) * 180. / M_PI;
  }
}
Esempio n. 5
0
static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) {
  struct ac_info_ * ac = get_ac_info(_ac_id);
  NavVerticalAutoThrottleMode(0.);
  NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.);
  float alpha = M_PI/2 - ac->course;
  float ca = cosf(alpha), sa = sinf(alpha);
  float x = ac->east - _distance*ca;
  float y = ac->north - _distance*sa;
  fly_to_xy(x, y);
#ifdef NAV_FOLLOW_PGAIN
  float s = (stateGetPositionEnu_f()->x - x)*ca + (stateGetPositionEnu_f()->y - y)*sa;
  nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN*s;
  nav_ground_speed_loop();
#endif
}
Esempio n. 6
0
/* 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;
  }
}
Esempio n. 7
0
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;
}
Esempio n. 8
0
int potential_task(void)
{

  uint8_t i;

  float ch = cosf(stateGetHorizontalSpeedDir_f());
  float sh = sinf(stateGetHorizontalSpeedDir_f());
  potential_force.east = 0.;
  potential_force.north = 0.;
  potential_force.alt = 0.;

  // compute control forces
  int8_t nb = 0;
  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 AC not responding for too long, continue, else compute force
    if (delta_t > CARROT) { continue; }
    else {
      float sha = sinf(ac->course);
      float cha = cosf(ac->course);
      float de = ac->east  + sha * delta_t - stateGetPositionEnu_f()->x;
      if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) { continue; }
      float dn = ac->north + cha * delta_t - stateGetPositionEnu_f()->y;
      if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) { continue; }
      float da = ac->alt + ac->climb * delta_t - stateGetPositionUtm_f()->alt;
      if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) { continue; }
      float dist = sqrtf(de * de + dn * dn + da * da);
      if (dist == 0.) { continue; }
      float dve = stateGetHorizontalSpeedNorm_f() * sh - ac->gspeed * sha;
      float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac->gspeed * cha;
      float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb;
      float scal = dve * de + dvn * dn + dva * da;
      if (scal < 0.) { continue; } // No risk of collision
      float d3 = dist * dist * dist;
      potential_force.east += scal * de / d3;
      potential_force.north += scal * dn / d3;
      potential_force.alt += scal * da / d3;
      ++nb;
    }
  }
  if (nb == 0) { return true; }
  //potential_force.east /= nb;
  //potential_force.north /= nb;
  //potential_force.alt /= nb;

  // set commands
  NavVerticalAutoThrottleMode(0.);

  // carrot
  float dx = -force_pos_gain * potential_force.east;
  float dy = -force_pos_gain * potential_force.north;
  desired_x += dx;
  desired_y += dy;
  // fly to desired
  fly_to_xy(desired_x, desired_y);

  // speed loop
  float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
  cruise += -force_speed_gain * (potential_force.north * ch + potential_force.east * sh);
  Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE);
  potential_force.speed = cruise;
  v_ctl_auto_throttle_cruise_throttle = cruise;

  // climb loop
  potential_force.climb = -force_climb_gain * potential_force.alt;
  BoundAbs(potential_force.climb, V_CTL_ALTITUDE_MAX_CLIMB);
  NavVerticalClimbMode(potential_force.climb);

  DOWNLINK_SEND_POTENTIAL(DefaultChannel, DefaultDevice, &potential_force.east, &potential_force.north,
                          &potential_force.alt, &potential_force.speed, &potential_force.climb);

  return true;
}
Esempio n. 9
0
int swarm_potential_task(void)
{
  struct EnuCoor_f speed_sp = {.x = 0., .y = 0., .z = 0.};

  // compute desired velocity
  int8_t nb = 0;
  uint8_t i;

  if (gps.fix == 0) {return 1;}
  struct UtmCoor_i my_pos;
  my_pos.zone = 0;
  utm_of_lla_i(&my_pos, &gps.lla_pos);    // TODO update height to hmsl

  for (i = 0; i < ti_acs_idx; i++) {
    if (ti_acs[i].ac_id == 0 || ti_acs[i].ac_id == AC_ID) { continue; }
    struct ac_info_ * ac = get_ac_info(ti_acs[i].ac_id);
    //float delta_t = ABS((int32_t)(gps.tow - ac->itow) / 1000.);
    // if AC not responding for too long, continue, else compute force
    //if(delta_t > 5) { continue; }

    float de = (ac->utm.east - my_pos.east) / 100.; // + sha * delta_t
    float dn = (ac->utm.north - my_pos.north) / 100.; // cha * delta_t
    float da = (ac->utm.alt - my_pos.alt) / 1000.; // ac->climb * delta_t   // currently wrong reference in other aircraft
    float dist2 = de * de + dn * dn;// + da * da;
    if (dist2 == 0.) {continue;}

    float dist = sqrtf(dist2);

    // potential force equation: x^2 - d0^3/x
    float force = dist2 - TARGET_DIST3 / dist;

    potential_force.east = (de * force) / dist;
    potential_force.north = (dn * force) / dist;
    potential_force.alt = (da * force) / dist;

    // set carrot
    // include speed of other vehicles for swarm cohesion
    speed_sp.x += force_hor_gain * potential_force.east;// + ac->gspeed * sinf(ac->course);
    speed_sp.y += force_hor_gain * potential_force.north;// + ac->gspeed * cosf(ac->course);
    speed_sp.z += force_climb_gain * potential_force.alt;// + ac->climb;

    nb++;

    //debug
    //potential_force.east  = de;
    //potential_force.north = dn;
    //potential_force.alt   = da;
  }

  // add waypoint force to get vehicle to waypoint
  if (use_waypoint) {
    struct EnuCoor_f my_enu = *stateGetPositionEnu_f();

    float de = waypoint_get_x(SP_WP) - my_enu.x;
    float dn = waypoint_get_y(SP_WP) - my_enu.y;
    float da = waypoint_get_alt(SP_WP) - my_enu.z;

    float dist2 = de * de + dn * dn;// + da * da;
    if (dist2 > 0.01) {   // add deadzone of 10cm from goal
      float dist = sqrtf(dist2);
      float force = 2 * dist2;

      // higher attractive potential to get to goal when close by
      if (dist < 1.) {
        force = 2 * dist;
      }

      potential_force.east  = force * de / dist;
      potential_force.north = force * dn / dist;
      potential_force.alt   = force * da / dist;

      speed_sp.x += force_hor_gain * potential_force.east;
      speed_sp.y += force_hor_gain * potential_force.north;
      speed_sp.z += force_climb_gain * potential_force.alt;

      potential_force.east  = de;
      potential_force.north = dn;
      potential_force.alt   = force;
      potential_force.speed   = speed_sp.x;
      potential_force.climb   = speed_sp.y;
    }
  }

  //potential_force.speed = speed_sp.x;
  //potential_force.climb = speed_sp.y;

  // limit commands
#ifdef GUIDANCE_H_REF_MAX_SPEED
  BoundAbs(speed_sp.x, GUIDANCE_H_REF_MAX_SPEED);
  BoundAbs(speed_sp.y, GUIDANCE_H_REF_MAX_SPEED);
  BoundAbs(speed_sp.z, GUIDANCE_H_REF_MAX_SPEED);
#endif

  potential_force.east  = speed_sp.x;
  potential_force.north = speed_sp.y;
  potential_force.alt   = speed_sp.z;

  autopilot_guided_move_ned(speed_sp.y, speed_sp.x, 0, 0);    // speed in enu

  return 1;
}