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; }
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); } }