test/src/harness_flight.cpp

   1 /* Copyright_License {
   2
   3   XCSoar Glide Computer - http://www.xcsoar.org/
   4   Copyright (C) 2000-2013 The XCSoar Project
   5   A detailed list of copyright holders can be found in the file "AUTHORS".
   6
   7   This program is free software; you can redistribute it and/or
   8   modify it under the terms of the GNU General Public License
   9   as published by the Free Software Foundation; either version 2
  10   of the License, or (at your option) any later version.
  11
  12   This program is distributed in the hope that it will be useful,
  13   but WITHOUT ANY WARRANTY; without even the implied warranty of
  14   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15   GNU General Public License for more details.
  16
  17   You should have received a copy of the GNU General Public License
  18   along with this program; if not, write to the Free Software
  19   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  20 }
  21 */
  22
  23 #include "harness_flight.hpp"
  24 #include "harness_airspace.hpp"
  25 #include "harness_wind.hpp"
  26 #include "TaskEventsPrint.hpp"
  27 #include "Util/AircraftStateFilter.hpp"
  28 #include "Replay/TaskAutoPilot.hpp"
  29 #include "Replay/AircraftSim.hpp"
  30 #include "Replay/TaskAccessor.hpp"
  31 #include "Engine/Waypoint/Waypoints.hpp"
  32 #include "Engine/Airspace/AirspaceAircraftPerformance.hpp"
  33 #include "OS/FileUtil.hpp"
  34
  35 #include <fstream>
  36
  37 fixed
  38 aat_min_time(int test_num)
  39 {
  40   OrderedTaskBehaviour beh;
  41   switch (test_num) {
  42   case 2:
  43     return fixed(3600 * 3.8);
  44   default:
  45     return beh.aat_min_time;
  46   }
  47 }
  48
  49 #include "Task/TaskManager.hpp"
  50 #include "Task/Factory/AbstractTaskFactory.hpp"
  51
  52 #define fixed_300 fixed(300)
  53
  54 class PrintTaskAutoPilot: public TaskAutoPilot
  55 {
  56 public:
  57   PrintTaskAutoPilot(const AutopilotParameters &_parms):
  58     TaskAutoPilot(_parms) {};
  59
  60 protected:
  61   virtual void OnManualAdvance() {
  62     if (verbose>1) {
  63       printf("# manual advance to %d\n",awp);
  64     }
  65   }
  66   virtual void OnModeChange() {
  67     if (verbose>1) {
  68       switch (acstate) {
  69       case Cruise:
  70         puts("# mode cruise");
  71         break;
  72       case Climb:
  73         puts("# mode climb");
  74         break;
  75       case FinalGlide:
  76         puts("# mode fg");
  77         break;
  78       }
  79     }
  80   }
  81   virtual void OnClose() {
  82     WaitPrompt();
  83   }
  84 };
  85
  86 TestFlightResult
  87 run_flight(TaskManager &task_manager, const AutopilotParameters &parms,
  88            const int n_wind, const double speed_factor)
  89 {
  90   return run_flight(TestFlightComponents(), task_manager, parms, n_wind,
  91                     speed_factor);
  92 }
  93
  94 TestFlightResult
  95 run_flight(TestFlightComponents components, TaskManager &task_manager,
  96            const AutopilotParameters &parms, const int n_wind,
  97            const double speed_factor)
  98 {
  99   AircraftStateFilter *aircraft_filter = components.aircraft_filter;
 100   Airspaces *airspaces = components.airspaces;
 101
 102   TestFlightResult result;
 103
 104   TaskAccessor ta(task_manager, fixed_300);
 105   PrintTaskAutoPilot autopilot(parms);
 106   AircraftSim aircraft;
 107
 108   autopilot.SetDefaultLocation(GeoPoint(Angle::Degrees(1), Angle::Degrees(0)));
 109
 110   unsigned print_counter=0;
 111   if (n_wind)
 112     aircraft.SetWind(wind_to_mag(n_wind), wind_to_dir(n_wind));
 113
 114   autopilot.SetSpeedFactor(fixed(speed_factor));
 115
 116   Directory::Create(_T("output/results"));
 117   std::ofstream f4("output/results/res-sample.txt");
 118   std::ofstream f5("output/results/res-sample-filtered.txt");
 119
 120   bool do_print = verbose;
 121   bool first = true;
 122
 123   static const fixed fixed_10(10);
 124
 125   const AirspaceAircraftPerformance perf(task_manager.GetGlidePolar());
 126
 127   if (aircraft_filter)
 128     aircraft_filter->Reset(aircraft.GetState());
 129
 130   autopilot.Start(ta);
 131   aircraft.Start(autopilot.location_start, autopilot.location_previous,
 132                  parms.start_alt);
 133
 134   AirspaceWarningManager *airspace_warnings;
 135   if (airspaces) {
 136     airspace_warnings = new AirspaceWarningManager(*airspaces);
 137     airspace_warnings->Reset(aircraft.GetState());
 138   } else {
 139     airspace_warnings = NULL;
 140   }
 141
 142   do {
 143
 144     if ((task_manager.GetActiveTaskPointIndex() == 1) &&
 145         first && (task_manager.GetStats().total.time_elapsed > fixed_10)) {
 146       result.time_remaining = (double)task_manager.GetStats().total.time_remaining;
 147       first = false;
 148
 149       result.time_planned = (double)task_manager.GetStats().total.time_planned;
 150
 151       if (verbose > 1) {
 152         printf("# time remaining %g\n", result.time_remaining);
 153         printf("# time planned %g\n", result.time_planned);
 154       }
 155     }
 156
 157     if (do_print) {
 158       PrintHelper::taskmanager_print(task_manager, aircraft.GetState());
 159
 160       const AircraftState state = aircraft.GetState();
 161       f4 << state.time << " "
 162          <<  state.location.longitude << " "
 163          <<  state.location.latitude << " "
 164          <<  state.altitude << "\n";
 165
 166       f4.flush();
 167       if (aircraft_filter) {
 168         f5 << aircraft_filter->GetSpeed() << " "
 169            << aircraft_filter->GetBearing() << " "
 170            << aircraft_filter->GetClimbRate() << "\n";
 171         f5.flush();
 172       }
 173     }
 174
 175     if (airspaces) {
 176       scan_airspaces(aircraft.GetState(), *airspaces, perf,
 177                      do_print,
 178                      autopilot.GetTarget(ta));
 179     }
 180     if (airspace_warnings) {
 181       if (verbose > 1) {
 182         bool warnings_updated = airspace_warnings->Update(aircraft.GetState(),
 183                                                           task_manager.GetGlidePolar(),
 184                                                           task_manager.GetStats(),
 185                                                           false, 1);
 186         if (warnings_updated) {
 187           printf("# airspace warnings updated, size %d\n",
 188                  (int)airspace_warnings->size());
 189           print_warnings(*airspace_warnings);
 190           WaitPrompt();
 191         }
 192       }
 193     }
 194
 195     n_samples++;
 196
 197     do_print = (++print_counter % output_skip == 0) && verbose;
 198
 199     if (aircraft_filter)
 200       aircraft_filter->Update(aircraft.GetState());
 201
 202     autopilot.UpdateState(ta, aircraft.GetState());
 203     aircraft.Update(autopilot.heading);
 204
 205     {
 206       const AircraftState state = aircraft.GetState();
 207       const AircraftState state_last = aircraft.GetLastState();
 208       task_manager.Update(state, state_last);
 209       task_manager.UpdateIdle(state);
 210       task_manager.UpdateAutoMC(state, fixed(0));
 211     }
 212
 213   } while (autopilot.UpdateAutopilot(ta, aircraft.GetState(), aircraft.GetLastState()));
 214
 215   if (verbose) {
 216     PrintHelper::taskmanager_print(task_manager, aircraft.GetState());
 217
 218     const AircraftState state = aircraft.GetState();
 219     f4 << state.time << " "
 220        <<  state.location.longitude << " "
 221        <<  state.location.latitude << " "
 222        <<  state.altitude << "\n";
 223     f4 << "\n";
 224     f4.flush();
 225     task_report(task_manager, "end of task\n");
 226   }
 227   WaitPrompt();
 228
 229   result.time_elapsed = (double)task_manager.GetStats().total.time_elapsed;
 230   result.time_planned = (double)task_manager.GetStats().total.time_planned;
 231   result.calc_cruise_efficiency = (double)task_manager.GetStats().cruise_efficiency;
 232   result.calc_effective_mc = (double)task_manager.GetStats().effective_mc;
 233
 234   if (verbose)
 235     PrintDistanceCounts();
 236
 237   if (airspace_warnings)
 238     delete airspace_warnings;
 239
 240   result.result = true;
 241   return result;
 242 }
 243
 244 TestFlightResult
 245 test_flight(int test_num, int n_wind, const double speed_factor,
 246             const bool auto_mc)
 247 {
 248   return test_flight(TestFlightComponents(), test_num, n_wind, speed_factor,
 249                      auto_mc);
 250 }
 251
 252 TestFlightResult
 253 test_flight(TestFlightComponents components, int test_num, int n_wind,
 254             const double speed_factor, const bool auto_mc)
 255 {
 256   // multipurpose flight test
 257
 258   GlidePolar glide_polar(fixed(2));
 259   Waypoints waypoints;
 260   SetupWaypoints(waypoints);
 261
 262   if (verbose)
 263     PrintDistanceCounts();
 264
 265   TaskBehaviour task_behaviour;
 266   task_behaviour.SetDefaults();
 267   task_behaviour.enable_trace = false;
 268   task_behaviour.auto_mc = auto_mc;
 269   task_behaviour.calc_glide_required = false;
 270   if ((test_num == 0) || (test_num == 2))
 271     task_behaviour.optimise_targets_bearing = false;
 272
 273   TaskManager task_manager(task_behaviour, waypoints);
 274
 275   TaskEventsPrint default_events(verbose);
 276   task_manager.SetTaskEvents(default_events);
 277   task_manager.SetGlidePolar(glide_polar);
 278
 279   OrderedTaskBehaviour otb = task_manager.GetOrderedTask().GetOrderedTaskBehaviour();
 280   otb.aat_min_time = aat_min_time(test_num);
 281   task_manager.SetOrderedTaskBehaviour(otb);
 282
 283   bool goto_target = false;
 284
 285   switch (test_num) {
 286   case 0:
 287   case 2:
 288   case 7:
 289     goto_target = true;
 290     break;
 291   };
 292   autopilot_parms.goto_target = goto_target;
 293   test_task(task_manager, waypoints, test_num);
 294
 295   waypoints.Clear(); // clear waypoints so abort wont do anything
 296
 297   return run_flight(components, task_manager, autopilot_parms, n_wind,
 298                     speed_factor);
 299 }
 300
 301 bool
 302 test_flight_times(int test_num, int n_wind)
 303 {
 304   // tests whether elapsed/planned times are consistent
 305   // there will be small error due to start location
 306
 307   TestFlightResult result = test_flight(test_num, n_wind);
 308   bool fine = result.result;
 309   const double t_rat = fabs(result.time_elapsed / result.time_planned - 1.0);
 310   fine &= t_rat < 0.02;
 311
 312   if ((verbose) || !fine) {
 313     printf("# time remaining %g\n", result.time_remaining);
 314     printf("# time elapsed %g\n", result.time_elapsed);
 315     printf("# time planned %g\n", result.time_planned);
 316     printf("# time ratio error (elapsed/planned) %g\n", t_rat);
 317   }
 318
 319   return fine;
 320 }