void intervalOfConflict(Daidalus& daa) {
bool comma = false;
std::string s="";
switch (format) {
case STANDARD:
for (int ac=1; ac <= daa.lastTrafficIndex(); ++ac) {
ConflictData conf = daa.detection(ac);
if (conf.conflict()) {
(*out) << "Predicted Loss of Well-Clear With " << daa.getAircraftState(ac).getId() <<
" in " << Units::str("s",conf.getTimeIn()) << " - "+Units::str("s",conf.getTimeOut()) << std::endl;
}
}
break;
case PVS:
s += "(: ";
for (int ac=1; ac <= daa.lastTrafficIndex(); ++ac) {
if (comma) {
s += ", ";
} else {
comma = true;
}
ConflictData conf = daa.detection(ac);
s += "("+FmPrecision(conf.getTimeIn(),precision)+","+FmPrecision(conf.getTimeOut(),precision)+")";
}
s += " :)";
(*out) << "%%% Time Interval of Violation:\n" << s << std::endl;
break;
default:
break;
}
}
void alerting(Daidalus& daa) {
std::string s="";
bool comma = false;
switch (format) {
case STANDARD:
for (int ac=1; ac <= daa.lastTrafficIndex(); ++ac) {
int alert_ac = daa.alerting(ac);
if (alert_ac > 0) {
(*out) << s << "Alert " << alert_ac << " with " << daa.getAircraftState(ac).getId() << std::endl;
}
}
(*out) << std::endl;
break;
case PVS:
s += "(: ";
for (int ac=1; ac <= daa.lastTrafficIndex(); ++ac) {
if (comma) {
s += ", ";
} else {
comma = true;
}
s += daa.alerting(ac);
}
s += " :)";
(*out) << "%%% Alerting:\n" << s << std::endl;
break;
default:
break;
}
}
void processTime(Daidalus& daa, const std::string& filename) {
daa.setCurrentTime(daa.getCurrentTime()+prj_t);
KinematicMultiBands kb;
daa.kinematicMultiBands(kb);
header(daa,kb,filename);
bands(kb);
intervalOfConflict(daa);
alerting(daa);
}
void printDetection(Daidalus& daa) {
// Aircraft at index 0 is ownship
for (int ac_idx=1; ac_idx <= daa.lastTrafficIndex(); ++ac_idx) {
double t2los = daa.timeToViolation(ac_idx);
if (t2los >= 0) {
std::cout << "Predicted Time to Loss of Well Clear with " << daa.getAircraftState(ac_idx).getId() << ": " <<
Fm2(t2los) << " [s]" << std::endl;
}
}
}
void printAlerts(Daidalus& daa) {
// Aircraft at index 0 is ownship
for (int ac_idx=1; ac_idx <= daa.lastTrafficIndex(); ++ac_idx) {
int alert = daa.alerting(ac_idx);
if (alert > 0) {
std::cout << "Alert Level " << alert << " with " <<
daa.getAircraftState(ac_idx).getId() << std::endl;
}
}
}
void printContours(Daidalus& daa) {
std::vector< std::vector<Position> > blobs = std::vector< std::vector<Position> >();
// Aircraft at index 0 is ownship
for (int ac_idx=1; ac_idx <= daa.lastTrafficIndex(); ++ac_idx) {
// Compute all contours
daa.horizontalContours(blobs,ac_idx);
for (unsigned int i=0; i < blobs.size(); ++i) {
std::cout << "Counter-clockwise Conflict Contour " << i << " with Aircraft " << daa.getAircraftState(ac_idx).getId() << ": " << std::endl;
for (unsigned int i=0; i < blobs[0].size(); ++i) {
std::cout << blobs[0][i].toString() << " ";
}
std::cout << std::endl;
}
}
}
void header(Daidalus& daa, KinematicMultiBands& bands, const std::string& filename) {
switch (format) {
case STANDARD:
break;
case PVS:
std::cout << "%%% File:\n" << filename << std::endl;
std::cout << "%%% Time:\n" << Units::str("s",daa.getCurrentTime()) << std::endl;
std::cout << "%%% Aircraft List:\n" << daa.aircraftListToPVS(precision) << std::endl;
std::cout << "%%% Most Urgent Aircraft:\n" << daa.mostUrgentAircraft().getId() << std::endl;
std::cout << "%%% Horizontal Epsilon:\n" << bands.core_.epsilonH() << std::endl;
std::cout << "%%% Vertical Epsilon:\n" << bands.core_.epsilonV() << std::endl;
break;
default:
break;
}
}
int main(int argc, const char* argv[]) {
DaidalusBatch walker;
int a;
std::string config = "";
std::string output = "";
std::string options = "";
ParameterData params;
int precision = 6;
for (a=1;a < argc && argv[a][0]=='-'; ++a) {
std::string arga = argv[a];
options += arga + " ";
if (walker.processOptions(argv,a)) {
++a;
options += walker.getOptionsString();
} if (arga == "--help" || arga == "-help" || arga == "-h") {
DaidalusBatch::printHelpMsg();
} else if (startsWith(arga,"--conf") || startsWith(arga,"-conf") || arga == "-c") {
config = argv[++a];
arga = argv[a];
options += arga + " ";
} else if (startsWith(arga,"--out") || startsWith(arga,"-out") || arga == "-o") {
output = argv[++a];
arga = argv[a];
} else if (arga == "--verbose" || arga == "-verbose" || arga == "-v") {
walker.verbose = true;
} else if (arga == "--raw" || arga == "-raw" || arga == "-r") {
walker.raw = true;
} else if (arga == "--pvs" || arga == "-pvs") {
walker.format = PVS;
} else if (startsWith(arga,"--proj") || startsWith(arga,"-proj")) {
++a;
walker.prj_t = Util::parse_double(argv[a]);
options += arga+" ";
} else if (startsWith(arga,"-") && arga.find('=') != std::string::npos) {
std::string keyval = arga.substr(arga.find_last_of('-')+1);
params.set(keyval);
} else if (argv[a][0] == '-') {
std::cout << "Invalid option: " << arga << std::endl;
exit(0);
}
}
std::vector<std::string> txtFiles = std::vector<std::string>();
for (;a < argc; ++a) {
std::string arga(argv[a]);
txtFiles.push_back(arga);
}
if (txtFiles.empty()) {
walker.printHelpMsg();
}
std::ofstream fout;
if (output != "") {
fout.open(output.c_str());
walker.out = &fout;
}
Daidalus daa;
if (config != "") {
daa.parameters.loadFromFile(config);
}
if (params.size() > 0) {
daa.parameters.setParameters(params);
}
switch (walker.format) {
case STANDARD:
if (walker.verbose) {
(*walker.out) << "# " << Daidalus::release() << std::endl;
(*walker.out) << "# Options: " << options << std::endl;
(*walker.out) << "#\n" << daa.toString() << "#\n" << std::endl;
}
break;
case PVS:
(*walker.out) << "%%% " << Daidalus::release() << std::endl;
(*walker.out) << "%%% Options: " << options << std::endl;
(*walker.out) << "%%% Parameters:\n"+daa.parameters.toPVS(precision) << std::endl;
break;
default:
break;
}
for (unsigned int i=0; i < txtFiles.size(); ++i) {
std::string filename(txtFiles[i]);
switch (walker.format) {
case STANDARD:
(*walker.out) << "# File: "<< filename << std::endl;
break;
case PVS:
(*walker.out) << "%%% File: " << filename << std::endl;
break;
default:
break;
}
walker.processFile(filename,daa);
}
if (output != "") {
fout.close();
}
}//main
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;
}
int main(int argc, char* argv[]) {
std::cout << "##" << std::endl;
std::cout << "## " << Daidalus::release() << std::endl;
std::cout << "##\n" << std::endl;
// Create an object of type Daidalus for a well-clear volume based on TAUMOD
Daidalus daa;
// Save default parameters
std::string default_parameters = "default_parameters.txt";
daa.parameters.saveToFile(default_parameters);
std::cout << "Default parameters written to file " << default_parameters << "\n" << std::endl;
// By default, DAIDALUS is configured to unbuffered WC, instantaneous bands.
// Uncomment one of the following lines to get buffered WC, kinematic bands.
// daa.set_Buffered_WC_SC_228_MOPS(false); // Turn rate 1.5 [deg/s]
// daa.set_Buffered_WC_SC_228_MOPS(true); // Turn rate 3.0 [deg/s]
// If needed, load parameters from a configuration file. In that case,
// uncomment the following line.
// daa.parameters.loadFromFile("my_parameters.txt");
double t = 0.0;
// for all times t (in this example, only one time step is illustrated)
// Add ownship state at time t
Position so = Position::makeLatLonAlt(33.95,"deg", -96.7,"deg", 8700.0,"ft");
Velocity vo = Velocity::makeTrkGsVs(206.0,"deg", 151.0,"knot", 0.0,"fpm");
daa.setOwnshipState("ownship",so,vo,t);
// Add all traffic states at time t
// ... some traffic ...
Position si = Position::makeLatLonAlt(33.86191658,"deg", -96.73272601,"deg", 9000.0,"ft");
Velocity vi = Velocity::makeTrkGsVs(0.0,"deg", 210.0,"knot", 0,"fpm");
daa.addTrafficState("ith-intruder",si,vi);
// ... more traffic ...
// Set wind information
Velocity wind = Velocity::makeTrkGsVs(45,"deg", 10,"knot", 0,"fpm");
daa.setWindField(wind);
// Print information about the Daidalus Object
std::cout << "Number of Aircraft: " << daa.numberOfAircraft() << std::endl;
std::cout << "Last Aircraft Index: " << daa.lastTrafficIndex() << std::endl;
std::cout << std::endl;
// Detect conflicts with every traffic aircraft
printDetection(daa);
// Call alerting logic for each traffic aircraft.
printAlerts(daa);
// Define multi bands object
KinematicMultiBands bands;
// Compute kinematic bands
daa.kinematicMultiBands(bands);
printBands(daa,bands);
// Print points of well-clear violation contours, i.e., blobs
printContours(daa);
// continue with next time step
}
