test/src/test_replay_task.cpp

   1 #include "test_debug.hpp"
   2 #include "harness_aircraft.hpp"
   3 #include "TaskEventsPrint.hpp"
   4 #include "Replay/IgcReplay.hpp"
   5 #include "Task/TaskManager.hpp"
   6 #include "Engine/Navigation/Aircraft.hpp"
   7 #include "Engine/Task/Ordered/OrderedTask.hpp"
   8 #include "Computer/FlyingComputer.hpp"
   9 #include "NMEA/FlyingState.hpp"
  10 #include "OS/ConvertPathName.hpp"
  11 #include "OS/FileUtil.hpp"
  12 #include "IO/FileLineReader.hpp"
  13 #include "Task/Deserialiser.hpp"
  14 #include "XML/DataNodeXML.hpp"
  15 #include "NMEA/Info.hpp"
  16 #include "Engine/Waypoint/Waypoints.hpp"
  17
  18 #include <fstream>
  19
  20 static OrderedTask* task_load(OrderedTask* task) {
  21   PathName szFilename(task_file.c_str());
  22   DataNode *root = DataNodeXML::Load(szFilename);
  23   if (!root)
  24     return NULL;
  25
  26   Deserialiser des(*root);
  27   des.Deserialise(*task);
  28   if (task->CheckTask()) {
  29     delete root;
  30     return task;
  31   }
  32   delete task;
  33   delete root;
  34   return NULL;
  35 }
  36
  37 class ReplayLoggerSim: public IgcReplay
  38 {
  39 public:
  40   ReplayLoggerSim(NLineReader *reader)
  41     :IgcReplay(reader),
  42      started(false) {}
  43
  44   AircraftState state;
  45
  46   void print(std::ostream &f) {
  47     f << (double)state.time << " "
  48       <<  (double)state.location.longitude.Degrees() << " "
  49       <<  (double)state.location.latitude.Degrees() << " "
  50       <<  (double)state.altitude << "\n";
  51   }
  52   bool started;
  53
  54 protected:
  55   virtual void OnReset() {}
  56   virtual void OnStop() {}
  57
  58   void OnAdvance(const GeoPoint &loc,
  59                   const fixed speed, const Angle bearing,
  60                   const fixed alt, const fixed baroalt, const fixed t) {
  61
  62     state.location = loc;
  63     state.ground_speed = speed;
  64     state.track = bearing;
  65     state.altitude = alt;
  66     state.time = t;
  67     if (positive(t)) {
  68       started = true;
  69     }
  70   }
  71 };
  72
  73 static bool
  74 test_replay()
  75 {
  76   Directory::Create(_T("output/results"));
  77   std::ofstream f("output/results/res-sample.txt");
  78
  79   GlidePolar glide_polar(fixed(4.0));
  80   Waypoints waypoints;
  81   AircraftState state_last;
  82
  83   TaskBehaviour task_behaviour;
  84   task_behaviour.SetDefaults();
  85   task_behaviour.auto_mc = true;
  86   task_behaviour.enable_trace = false;
  87
  88   TaskManager task_manager(task_behaviour, waypoints);
  89
  90   TaskEventsPrint default_events(verbose);
  91   task_manager.SetTaskEvents(default_events);
  92
  93   glide_polar.SetBallast(fixed(1.0));
  94
  95   task_manager.SetGlidePolar(glide_polar);
  96
  97   OrderedTask* blank = new OrderedTask(task_manager.GetTaskBehaviour());
  98
  99   OrderedTask* t = task_load(blank);
 100   if (t) {
 101     task_manager.Commit(*t);
 102     task_manager.Resume();
 103   } else {
 104     return false;
 105   }
 106
 107   // task_manager.get_task_advance().get_advance_state() = TaskAdvance::AUTO;
 108
 109   FileLineReaderA *reader = new FileLineReaderA(replay_file.c_str());
 110   if (reader->error()) {
 111     delete reader;
 112     return false;
 113   }
 114
 115   ReplayLoggerSim sim(reader);
 116   sim.state.netto_vario = fixed(0);
 117
 118   bool do_print = verbose;
 119   unsigned print_counter=0;
 120
 121   NMEAInfo basic;
 122   basic.Reset();
 123
 124   while (sim.Update(basic) && !sim.started) {
 125   }
 126   state_last = sim.state;
 127
 128   sim.state.wind.norm = fixed(7);
 129   sim.state.wind.bearing = Angle::Degrees(330);
 130
 131   fixed time_last = sim.state.time;
 132
 133 //  uncomment this to manually go to first tp
 134 //  task_manager.incrementActiveTaskPoint(1);
 135
 136   FlyingComputer flying_computer;
 137   flying_computer.Reset();
 138
 139   FlyingState flying_state;
 140   flying_state.Reset();
 141
 142   while (sim.Update(basic)) {
 143     if (sim.state.time>time_last) {
 144
 145       n_samples++;
 146
 147       flying_computer.Compute(glide_polar.GetVTakeoff(),
 148                               sim.state, sim.state.time - time_last,
 149                               flying_state);
 150       sim.state.flying = flying_state.flying;
 151
 152       task_manager.Update(sim.state, state_last);
 153       task_manager.UpdateIdle(sim.state);
 154       task_manager.UpdateAutoMC(sim.state, fixed(0));
 155       task_manager.SetTaskAdvance().SetArmed(true);
 156
 157       state_last = sim.state;
 158
 159       if (verbose>1) {
 160         sim.print(f);
 161         f.flush();
 162       }
 163       if (do_print) {
 164         PrintHelper::taskmanager_print(task_manager, sim.state);
 165       }
 166       do_print = (++print_counter % output_skip ==0) && verbose;
 167     }
 168     time_last = sim.state.time;
 169   };
 170
 171   if (verbose) {
 172     PrintDistanceCounts();
 173     printf("# task elapsed %d (s)\n", (int)task_manager.GetStats().total.time_elapsed);
 174     printf("# task speed %3.1f (kph)\n", (int)task_manager.GetStats().total.travelled.GetSpeed()*3.6);
 175     printf("# travelled distance %4.1f (km)\n",
 176            (double)task_manager.GetStats().total.travelled.GetDistance()/1000.0);
 177     printf("# scored distance %4.1f (km)\n",
 178            (double)task_manager.GetStats().distance_scored/1000.0);
 179     if (positive(task_manager.GetStats().total.time_elapsed)) {
 180       printf("# scored speed %3.1f (kph)\n",
 181              (double)task_manager.GetStats().distance_scored/(double)task_manager.GetStats().total.time_elapsed*3.6);
 182     }
 183   }
 184   return true;
 185 }
 186
 187
 188 int main(int argc, char** argv)
 189 {
 190   output_skip = 60;
 191
 192   replay_file = "test/data/apf-bug554.igc";
 193   task_file = "test/data/apf-bug554.tsk";
 194
 195   if (!ParseArgs(argc,argv)) {
 196     return 0;
 197   }
 198
 199   plan_tests(1);
 200
 201   ok(test_replay(),"replay task",0);
 202
 203   return exit_status();
 204 }
 205