int KinematicAltBands::first_band_alt_generic(Detection3D* conflict_det, Detection3D* recovery_det,
double B, double T, double B2, double T2,
const TrafficState& ownship, const std::vector<TrafficState>& traffic, bool dir, bool green) {
int upper = (int)(dir ? std::floor((max_val(ownship)-min_val(ownship))/get_step())+1 :
std::floor((ownship.altitude()-min_val(ownship))/get_step()));
int lower = dir ? (int)(std::ceil(ownship.altitude()-min_val(ownship))/get_step()) : 0;
if (ownship.altitude() < min_val(ownship) || ownship.altitude() > max_val(ownship)) {
return -1;
} else {
return first_nat(lower,upper,dir,conflict_det,recovery_det,B,T,B2,T2,ownship,traffic,green);
}
}
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);
}
}
void printBands(Daidalus& daa, KinematicMultiBands& bands) {
bool nowind = daa.getWindField().isZero();
TrafficState own = daa.getOwnshipState();
std::string trkstr = nowind ? "Track" : "Heading";
std::string gsstr = nowind ? "Ground Speed" : "Airspeed";
std::cout << std::endl;
for (int alert_level = 1; alert_level <= daa.parameters.alertor.mostSevereAlertLevel(); ++alert_level) {
std::cout << "Conflict Aircraft for Alert Level " << Fmi(alert_level) << ": " <<
TrafficState::listToString(bands.conflictAircraft(alert_level)) << std::endl;
}
std::cout << std::endl;
// Track/Heading
double trk_deg = own.track("deg");
std::cout << "Ownship " << trkstr << ": "+Fm2(trk_deg) << " [deg]" << std::endl;
std::cout << "Region of Current " << trkstr+": " <<
BandsRegion::to_string(bands.regionOfTrack(trk_deg,"deg")) << std::endl;
std::cout << trkstr << " Bands [deg,deg]" << std::endl;
for (int i=0; i < bands.trackLength(); ++i) {
Interval ii = bands.track(i,"deg");
std::cout << " " << BandsRegion::to_string(bands.trackRegion(i)) << ":\t" << ii.toString(2) << std::endl;
}
for (int alert_level = 1; alert_level <= daa.parameters.alertor.mostSevereAlertLevel(); ++alert_level) {
std::cout << "Peripheral " << trkstr << " Aircraft for Alert Level " << Fmi(alert_level) << ": " <<
TrafficState::listToString(bands.peripheralTrackAircraft(alert_level)) << std::endl;
}
std::cout << trkstr << " Resolution (right): " << num2str(bands.trackResolution(true,"deg"),"deg") << std::endl;
std::cout << trkstr << " Resolution (left): " << num2str(bands.trackResolution(false,"deg"),"deg") << std::endl;
std::cout << "Preferred "+trkstr+" Direction: ";
if (bands.preferredTrackDirection()) {
std::cout << "right" << std::endl;
} else {
std::cout << "left" << std::endl;
}
std::cout << "Time to " << trkstr << " Recovery: " << num2str(bands.timeToTrackRecovery(),"s") << std::endl;
// Ground Speed/Air Speed
double gs_knot = own.groundSpeed("knot");
std::cout << "Ownship " << gsstr << ": "+Fm2(gs_knot) << " [knot]" << std::endl;
std::cout << "Region of Current " << gsstr+": " <<
BandsRegion::to_string(bands.regionOfGroundSpeed(gs_knot,"knot")) << std::endl;
std::cout << gsstr << " Bands [knot,knot]:" << std::endl;
for (int i=0; i < bands.groundSpeedLength(); ++i) {
Interval ii = bands.groundSpeed(i,"knot");
std::cout << " " << BandsRegion::to_string(bands.groundSpeedRegion(i)) << ":\t" << ii.toString(2) << std::endl;
}
for (int alert_level = 1; alert_level <= daa.parameters.alertor.mostSevereAlertLevel(); ++alert_level) {
std::cout << "Peripheral " << gsstr << " Aircraft for Alert Level " << Fmi(alert_level) << ": " <<
TrafficState::listToString(bands.peripheralGroundSpeedAircraft(alert_level)) << std::endl;
}
std::cout << gsstr << " Resolution (up): " << num2str(bands.groundSpeedResolution(true,"knot"),"knot") << std::endl;
std::cout << gsstr << " Resolution (down): " << num2str(bands.groundSpeedResolution(false,"knot"),"knot") << std::endl;
std::cout << "Preferred "+gsstr+" Direction: ";
if (bands.preferredGroundSpeedDirection()) {
std::cout << "up" << std::endl;
} else {
std::cout << "down" << std::endl;
}
std::cout << "Time to " << gsstr << " Recovery: " << num2str(bands.timeToGroundSpeedRecovery(),"s") << std::endl;
// Vertical Speed
double vs_fpm = own.verticalSpeed("fpm");
std::cout << "Ownship Vertical Speed: "+Fm2(vs_fpm) << " [fpm]" << std::endl;
std::cout << "Region of Current Vertical Speed: " <<
BandsRegion::to_string(bands.regionOfVerticalSpeed(vs_fpm,"fpm")) << std::endl;
std::cout << "Vertical Speed Bands [fpm,fpm]:" << std::endl;
for (int i=0; i < bands.verticalSpeedLength(); ++i) {
Interval ii = bands.verticalSpeed(i,"fpm");
std::cout << " " << BandsRegion::to_string(bands.verticalSpeedRegion(i)) << ":\t" << ii.toString(2) << std::endl;
}
for (int alert_level = 1; alert_level <= daa.parameters.alertor.mostSevereAlertLevel(); ++alert_level) {
std::cout << "Peripheral Vertical Speed Aircraft for Alert Level " << Fmi(alert_level) << ": " <<
TrafficState::listToString(bands.peripheralVerticalSpeedAircraft(alert_level)) << std::endl;
}
std::cout << "Vertical Speed Resolution (up): " << num2str(bands.verticalSpeedResolution(true,"fpm"),"fpm") << std::endl;
std::cout << "Vertical Speed Resolution (down): " << num2str(bands.verticalSpeedResolution(false,"fpm"),"fpm") << std::endl;
std::cout << "Preferred Vertical Speed Direction: ";
if (bands.preferredVerticalSpeedDirection()) {
std::cout << "up" << std::endl;
} else {
std::cout << "down" << std::endl;
}
std::cout << "Time to Vertical Speed Recovery: " << num2str(bands.timeToVerticalSpeedRecovery(),"s") << std::endl;
// Altitude
double alt_ft = own.altitude("ft");
std::cout << "Ownship Altitude: "+Fm2(alt_ft) << " [ft]" << std::endl;
std::cout << "Region of Current Altitude: " <<
BandsRegion::to_string(bands.regionOfAltitude(alt_ft,"ft")) << std::endl;
std::cout << "Altitude Bands [ft,ft]:" << std::endl;
for (int i=0; i < bands.altitudeLength(); ++i) {
Interval ii = bands.altitude(i,"ft");
std::cout << " " << BandsRegion::to_string(bands.altitudeRegion(i)) << ":\t" << ii.toString(2) << std::endl;
}
for (int alert_level = 1; alert_level <= daa.parameters.alertor.mostSevereAlertLevel(); ++alert_level) {
std::cout << "Peripheral Altitude Aircraft for Alert Level " << Fmi(alert_level) << ": " <<
TrafficState::listToString(bands.peripheralAltitudeAircraft(alert_level)) << std::endl;
}
std::cout << "Altitude Resolution (up): " << num2str(bands.altitudeResolution(true,"ft"),"ft") << std::endl;
std::cout << "Altitude Resolution (down): " << num2str(bands.altitudeResolution(false,"ft"),"ft") << std::endl;
std::cout << "Preferred Altitude Direction: ";
if (bands.preferredAltitudeDirection()) {
std::cout << "up" << std::endl;
} else {
std::cout << "down" << std::endl;
}
std::cout << "Time to Altitude Recovery: " << num2str(bands.timeToAltitudeRecovery(),"s") << std::endl;
std::cout << std::endl;
// Last times to maneuver
for (int ac_idx=1; ac_idx <= daa.lastTrafficIndex(); ++ac_idx) {
TrafficState ac = daa.getAircraftState(ac_idx);
std::cout << "Last Times to Maneuver with Respect to " << ac.getId() << ":" << std::endl;
std::cout << " "+trkstr+" Maneuver: "+num2str(bands.lastTimeToTrackManeuver(ac),"s") << std::endl;
std::cout << " "+gsstr+" Maneuver: "+num2str(bands.lastTimeToGroundSpeedManeuver(ac),"s") << std::endl;
std::cout <<" Vertical Speed Maneuver: "+num2str(bands.lastTimeToVerticalSpeedManeuver(ac),"s") << std::endl;
std::cout <<" Altitude Maneuver: "+num2str(bands.lastTimeToAltitudeManeuver(ac),"s") << std::endl;
}
std::cout << std::endl;
}
double KinematicAltBands::own_val(const TrafficState& ownship) const {
return ownship.altitude();
}
