Ejemplo n.º 1
0
void KinematicIntegerBands::instantaneous_bands_combine(std::vector<Integerval>& l, Detection3D* conflict_det, Detection3D* recovery_det,
    double B, double T, double B2, double T2,
    int maxl, int maxr, const TrafficState& ownship, const std::vector<TrafficState>& traffic,
    const TrafficState& repac,
    int epsh, int epsv) {
  instantaneous_bands(l,conflict_det,recovery_det,B,T,B2,T2,false,maxl,ownship,traffic,repac,epsh,epsv);
  std::vector<Integerval> r = std::vector<Integerval>();
  instantaneous_bands(r,conflict_det,recovery_det,B,T,B2,T2,true,maxr,ownship,traffic,repac,epsh,epsv);
  neg(l);
  append_intband(l,r);
}
Ejemplo n.º 2
0
std::pair<Vect3, Velocity> KinematicTrkBands::trajectory(const TrafficState& ownship, double time, bool dir) const {
  std::pair<Position,Velocity> posvel;
  if (instantaneous_bands()) {
    double trk = ownship.getVelocity().trk()+(dir?1:-1)*j_step_*get_step();
    posvel = std::pair<Position,Velocity>(ownship.getPosition(),ownship.getVelocity().mkTrk(trk));
  } else {
    double gso = ownship.groundSpeed();
    double bank = turn_rate_ == 0 ? bank_angle_ : std::abs(Kinematics::bankAngle(gso,turn_rate_));
    double R = Kinematics::turnRadius(ownship.get_v().gs(), bank);
    posvel = ProjectedKinematics::turn(ownship.getPosition(),ownship.getVelocity(),time,R,dir);
  }
  return std::pair<Vect3, Velocity>(ownship.pos_to_s(posvel.first),ownship.vel_to_v(posvel.first,posvel.second));
}
Ejemplo n.º 3
0
std::pair<Vect3, Velocity> KinematicAltBands::trajectory(const TrafficState& ownship, double time, bool dir) const {
  double target_alt = min_val(ownship)+j_step_*get_step();
  std::pair<Position,Velocity> posvel;
  if (instantaneous_bands()) {
    posvel = std::pair<Position,Velocity>(ownship.getPosition().mkZ(target_alt),ownship.getVelocity().mkVs(0));
  } else {
    double tsqj = ProjectedKinematics::vsLevelOutTime(ownship.getPosition(),ownship.getVelocity(),vertical_rate_,
        target_alt,vertical_accel_)+time_step(ownship);
    if (time <= tsqj) {
      posvel = ProjectedKinematics::vsLevelOut(ownship.getPosition(), ownship.getVelocity(), time, vertical_rate_, target_alt, vertical_accel_);
    } else {
      Position npo = ownship.getPosition().linear(ownship.getVelocity(),time);
      posvel = std::pair<Position,Velocity>(npo.mkZ(target_alt),ownship.getVelocity().mkVs(0));
    }
  }
  return std::pair<Vect3,Velocity>(ownship.pos_to_s(posvel.first),ownship.vel_to_v(posvel.first,posvel.second));
}
Ejemplo n.º 4
0
bool KinematicAltBands::conflict_free_traj_step(Detection3D* conflict_det, Detection3D* recovery_det,
    double B, double T, double B2, double T2,
    const TrafficState& ownship, const std::vector<TrafficState>& traffic) const {
  bool trajdir = true;
  if (instantaneous_bands()) {
    return no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,0,ownship,traffic);
  } else {
    double tstep = time_step(ownship);
    double target_alt = min_val(ownship)+j_step_*get_step();
    Tuple5<double,double,double,double,double> tsqj = Kinematics::vsLevelOutTimes(ownship.altitude(),ownship.verticalSpeed(),
        vertical_rate_,target_alt,vertical_accel_,-vertical_accel_,true);
    double tsqj1 = tsqj.first+0;
    double tsqj2 = tsqj.second+0;
    double tsqj3 = tsqj.third+tstep;
    for (int i=0; i<=std::floor(tsqj1/tstep);++i) {
      double tsi = i*tstep;
      if ((B<=tsi && tsi<=T && any_los_aircraft(conflict_det,trajdir,tsi,ownship,traffic)) ||
          (recovery_det != NULL && B2 <= tsi && tsi <= T2 &&
              any_los_aircraft(recovery_det,trajdir,tsi,ownship,traffic))) {
        return false;
      }
    }
    if ((tsqj2>=B &&
        any_conflict_aircraft(conflict_det,B,std::min(T,tsqj2),trajdir,std::max(tsqj1,0.0),ownship,traffic)) ||
        (recovery_det != NULL && tsqj2>=B2 &&
            any_conflict_aircraft(recovery_det,B2,std::min(T2,tsqj2),trajdir,std::max(tsqj1,0.0),ownship,traffic))) {
      return false;
    }
    for (int i=(int)std::ceil(tsqj2/tstep); i<=std::floor(tsqj3/tstep);++i) {
      double tsi = i*tstep;
      if ((B<=tsi && tsi<=T && any_los_aircraft(conflict_det,trajdir,tsi,ownship,traffic)) ||
          (recovery_det != NULL && B2 <= tsi && tsi <= T2 &&
              any_los_aircraft(recovery_det,trajdir,tsi,ownship,traffic))) {
        return false;
      }
    }
    return no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,std::max(tsqj3,0.0),ownship,traffic);
  }
}