TestFlightResult
run_flight(TestFlightComponents components, TaskManager &task_manager,
const AutopilotParameters &parms, const int n_wind,
const double speed_factor)
{
AircraftStateFilter *aircraft_filter = components.aircraft_filter;
Airspaces *airspaces = components.airspaces;
TestFlightResult result;
TaskAccessor ta(task_manager, fixed_300);
PrintTaskAutoPilot autopilot(parms);
AircraftSim aircraft;
autopilot.SetDefaultLocation(GeoPoint(Angle::Degrees(1), Angle::Degrees(0)));
unsigned print_counter=0;
if (n_wind)
aircraft.SetWind(wind_to_mag(n_wind), wind_to_dir(n_wind));
autopilot.SetSpeedFactor(fixed(speed_factor));
Directory::Create(_T("output/results"));
std::ofstream f4("output/results/res-sample.txt");
std::ofstream f5("output/results/res-sample-filtered.txt");
bool do_print = verbose;
bool first = true;
static const fixed fixed_10(10);
const AirspaceAircraftPerformance perf(task_manager.GetGlidePolar());
if (aircraft_filter)
aircraft_filter->Reset(aircraft.GetState());
autopilot.Start(ta);
aircraft.Start(autopilot.location_start, autopilot.location_previous,
parms.start_alt);
AirspaceWarningManager *airspace_warnings;
if (airspaces) {
airspace_warnings = new AirspaceWarningManager(*airspaces);
airspace_warnings->Reset(aircraft.GetState());
} else {
airspace_warnings = NULL;
}
do {
if ((task_manager.GetActiveTaskPointIndex() == 1) &&
first && (task_manager.GetStats().total.time_elapsed > fixed_10)) {
result.time_remaining = (double)task_manager.GetStats().total.time_remaining_now;
first = false;
result.time_planned = (double)task_manager.GetStats().total.time_planned;
if (verbose > 1) {
printf("# time remaining %g\n", result.time_remaining);
printf("# time planned %g\n", result.time_planned);
}
}
if (do_print) {
PrintHelper::taskmanager_print(task_manager, aircraft.GetState());
const AircraftState state = aircraft.GetState();
f4 << state.time << " "
<< state.location.longitude << " "
<< state.location.latitude << " "
<< state.altitude << "\n";
f4.flush();
if (aircraft_filter) {
f5 << aircraft_filter->GetSpeed() << " "
<< aircraft_filter->GetBearing() << " "
<< aircraft_filter->GetClimbRate() << "\n";
f5.flush();
}
}
if (airspaces) {
scan_airspaces(aircraft.GetState(), *airspaces, perf,
do_print,
autopilot.GetTarget(ta));
}
if (airspace_warnings) {
if (verbose > 1) {
bool warnings_updated = airspace_warnings->Update(aircraft.GetState(),
task_manager.GetGlidePolar(),
task_manager.GetStats(),
false, 1);
if (warnings_updated) {
printf("# airspace warnings updated, size %d\n",
(int)airspace_warnings->size());
print_warnings(*airspace_warnings);
WaitPrompt();
}
}
}
n_samples++;
do_print = (++print_counter % output_skip == 0) && verbose;
if (aircraft_filter)
aircraft_filter->Update(aircraft.GetState());
autopilot.UpdateState(ta, aircraft.GetState());
aircraft.Update(autopilot.heading);
{
const AircraftState state = aircraft.GetState();
const AircraftState state_last = aircraft.GetLastState();
task_manager.Update(state, state_last);
task_manager.UpdateIdle(state);
task_manager.UpdateAutoMC(state, fixed(0));
}
} while (autopilot.UpdateAutopilot(ta, aircraft.GetState()));
if (verbose) {
PrintHelper::taskmanager_print(task_manager, aircraft.GetState());
const AircraftState state = aircraft.GetState();
f4 << state.time << " "
<< state.location.longitude << " "
<< state.location.latitude << " "
<< state.altitude << "\n";
f4 << "\n";
f4.flush();
task_report(task_manager, "end of task\n");
}
WaitPrompt();
result.time_elapsed = (double)task_manager.GetStats().total.time_elapsed;
result.time_planned = (double)task_manager.GetStats().total.time_planned;
result.calc_cruise_efficiency = (double)task_manager.GetStats().cruise_efficiency;
result.calc_effective_mc = (double)task_manager.GetStats().effective_mc;
if (verbose)
PrintDistanceCounts();
if (airspace_warnings)
delete airspace_warnings;
result.result = true;
return result;
}
static bool
test_route(const unsigned n_airspaces, const RasterMap& map)
{
Airspaces airspaces;
setup_airspaces(airspaces, map.GetMapCenter(), n_airspaces);
{
std::ofstream fout("results/terrain.txt");
unsigned nx = 100;
unsigned ny = 100;
GeoPoint origin(map.GetMapCenter());
for (unsigned i = 0; i < nx; ++i) {
for (unsigned j = 0; j < ny; ++j) {
fixed fx = (fixed)i / (nx - 1) * fixed(2.0) - fixed_one;
fixed fy = (fixed)j / (ny - 1) * fixed(2.0) - fixed_one;
GeoPoint x(origin.longitude + Angle::Degrees(fixed(0.2) + fixed(0.7) * fx),
origin.latitude + Angle::Degrees(fixed(0.9) * fy));
short h = map.GetInterpolatedHeight(x);
fout << x.longitude.Degrees() << " " << x.latitude.Degrees()
<< " " << h << "\n";
}
fout << "\n";
}
fout << "\n";
}
{
// local scope, see what happens when we go out of scope
GeoPoint p_start(Angle::Degrees(fixed(-0.3)), Angle::Degrees(fixed(0.0)));
p_start += map.GetMapCenter();
GeoPoint p_dest(Angle::Degrees(fixed(0.8)), Angle::Degrees(fixed(-0.7)));
p_dest += map.GetMapCenter();
AGeoPoint loc_start(p_start, RoughAltitude(map.GetHeight(p_start) + 100));
AGeoPoint loc_end(p_dest, RoughAltitude(map.GetHeight(p_dest) + 100));
AircraftState state;
GlidePolar glide_polar(fixed(0.1));
AirspaceAircraftPerformanceGlide perf(glide_polar);
GeoVector vec(loc_start, loc_end);
fixed range = fixed(10000) + vec.distance / 2;
state.location = loc_start;
state.altitude = loc_start.altitude;
{
Airspaces as_route(airspaces, false);
// dummy
// real one, see if items changed
as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), range);
int size_1 = as_route.size();
if (verbose)
printf("# route airspace size %d\n", size_1);
as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), fixed_one);
int size_2 = as_route.size();
if (verbose)
printf("# route airspace size %d\n", size_2);
ok(size_2 < size_1, "shrink as", 0);
// go back
as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_end), range);
int size_3 = as_route.size();
if (verbose)
printf("# route airspace size %d\n", size_3);
ok(size_3 >= size_2, "grow as", 0);
// and again
as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), range);
int size_4 = as_route.size();
if (verbose)
printf("# route airspace size %d\n", size_4);
ok(size_4 >= size_3, "grow as", 0);
scan_airspaces(state, as_route, perf, true, loc_end);
}
// try the solver
SpeedVector wind(Angle::Degrees(fixed(0)), fixed(0.0));
GlidePolar polar(fixed_one);
GlideSettings settings;
settings.SetDefaults();
AirspaceRoute route(airspaces);
route.UpdatePolar(settings, polar, polar, wind);
route.SetTerrain(&map);
RoutePlannerConfig config;
config.mode = RoutePlannerConfig::Mode::BOTH;
bool sol = false;
for (int i = 0; i < NUM_SOL; i++) {
loc_end.latitude += Angle::Degrees(fixed(0.1));
loc_end.altitude = map.GetHeight(loc_end) + 100;
route.Synchronise(airspaces, loc_start, loc_end);
if (route.Solve(loc_start, loc_end, config)) {
sol = true;
if (verbose) {
PrintHelper::print_route(route);
}
} else {
if (verbose) {
printf("# fail\n");
}
sol = false;
}
char buffer[80];
sprintf(buffer, "route %d solution", i);
ok(sol, buffer, 0);
}
}
return true;
}
bool run_flight(TaskManager &task_manager,
const AutopilotParameters &parms,
const int n_wind,
const double speed_factor)
{
DirectTaskAccessor ta(task_manager);
PrintTaskAutoPilot autopilot(parms);
AircraftSim aircraft;
autopilot.set_default_location(GeoPoint(Angle::degrees(fixed(1.0)), Angle::degrees(fixed(0.0))));
unsigned print_counter=0;
if (n_wind)
aircraft.set_wind(wind_to_mag(n_wind), wind_to_dir(n_wind));
autopilot.set_speed_factor(fixed(speed_factor));
std::ofstream f4("results/res-sample.txt");
std::ofstream f5("results/res-sample-filtered.txt");
bool do_print = verbose;
bool first = true;
time_elapsed = 0.0;
time_planned = 1.0;
time_remaining = 0;
calc_cruise_efficiency = 1.0;
calc_effective_mc = 1.0;
static const fixed fixed_10(10);
AirspaceAircraftPerformanceGlide perf(task_manager.get_glide_polar());
if (aircraft_filter)
aircraft_filter->Reset(aircraft.get_state());
autopilot.Start(ta);
aircraft.Start(autopilot.location_start, autopilot.location_previous,
parms.start_alt);
if (airspaces) {
airspace_warnings =
new AirspaceWarningManager(*airspaces, task_manager);
airspace_warnings->reset(aircraft.get_state());
}
do {
if ((task_manager.getActiveTaskPointIndex() == 1) &&
first && (task_manager.get_stats().total.time_elapsed > fixed_10)) {
time_remaining = task_manager.get_stats().total.time_remaining;
first = false;
time_planned = task_manager.get_stats().total.time_planned;
if (verbose > 1) {
printf("# time remaining %g\n", time_remaining);
printf("# time planned %g\n", time_planned);
}
}
if (do_print) {
PrintHelper::taskmanager_print(task_manager, aircraft.get_state());
const AircraftState state = aircraft.get_state();
f4 << state.time << " "
<< state.location.Longitude << " "
<< state.location.Latitude << " "
<< state.altitude << "\n";
f4.flush();
if (aircraft_filter) {
f5 << aircraft_filter->GetSpeed() << " "
<< aircraft_filter->GetBearing() << " "
<< aircraft_filter->GetClimbRate() << "\n";
f5.flush();
}
}
if (airspaces) {
scan_airspaces(aircraft.get_state(), *airspaces, perf,
do_print,
autopilot.target(ta));
}
if (airspace_warnings) {
if (verbose > 1) {
bool warnings_updated = airspace_warnings->update(aircraft.get_state(),
false, 1);
if (warnings_updated) {
printf("# airspace warnings updated, size %d\n",
(int)airspace_warnings->size());
print_warnings();
wait_prompt();
}
}
}
n_samples++;
do_print = (++print_counter % output_skip == 0) && verbose;
if (aircraft_filter)
aircraft_filter->Update(aircraft.get_state());
autopilot.update_state(ta, aircraft.get_state());
aircraft.Update(autopilot.heading);
{
const AircraftState state = aircraft.get_state();
const AircraftState state_last = aircraft.get_state_last();
task_manager.update(state, state_last);
task_manager.update_idle(state);
task_manager.update_auto_mc(state, fixed_zero);
}
} while (autopilot.update_autopilot(ta, aircraft.get_state(), aircraft.get_state_last()));
aircraft.Stop();
autopilot.Stop();
if (verbose) {
PrintHelper::taskmanager_print(task_manager, aircraft.get_state());
const AircraftState state = aircraft.get_state();
f4 << state.time << " "
<< state.location.Longitude << " "
<< state.location.Latitude << " "
<< state.altitude << "\n";
f4 << "\n";
f4.flush();
task_report(task_manager, "end of task\n");
}
wait_prompt();
time_elapsed = task_manager.get_stats().total.time_elapsed;
time_planned = task_manager.get_stats().total.time_planned;
calc_cruise_efficiency = task_manager.get_stats().cruise_efficiency;
calc_effective_mc = task_manager.get_stats().effective_mc;
if (verbose)
distance_counts();
if (airspace_warnings)
delete airspace_warnings;
return true;
}
