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)); }
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 KinematicTrkBands::time_step(const TrafficState& ownship) const { double gso = ownship.groundSpeed(); double omega = turn_rate_ == 0 ? Kinematics::turnRate(gso,bank_angle_) : turn_rate_; return get_step()/omega; }