test/src/TestOrderedTask.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 "Engine/GlideSolvers/GlidePolar.hpp"
  24 #include "Engine/Task/TaskEvents.hpp"
  25 #include "Engine/Task/Ordered/OrderedTaskBehaviour.hpp"
  26 #include "Engine/Task/Ordered/OrderedTask.hpp"
  27 #include "Engine/Task/Ordered/Points/StartPoint.hpp"
  28 #include "Engine/Task/Ordered/Points/FinishPoint.hpp"
  29 #include "Engine/Task/Ordered/Points/ASTPoint.hpp"
  30 #include "Engine/Task/ObservationZones/LineSectorZone.hpp"
  31
  32 #ifdef FIXED_MATH
  33 #define ACCURACY 100
  34 #else
  35 #define ACCURACY 500
  36 #endif
  37
  38 #include "TestUtil.hpp"
  39
  40 static TaskBehaviour task_behaviour;
  41 static OrderedTaskBehaviour ordered_task_behaviour;
  42 static GlidePolar glide_polar(fixed(0));
  43
  44 static GeoPoint
  45 MakeGeoPoint(double longitude, double latitude)
  46 {
  47   return GeoPoint(Angle::Degrees(longitude),
  48                   Angle::Degrees(latitude));
  49 }
  50
  51 static Waypoint
  52 MakeWaypoint(Waypoint wp, double altitude)
  53 {
  54   wp.elevation = fixed(altitude);
  55   return wp;
  56 }
  57
  58 static Waypoint
  59 MakeWaypoint(double longitude, double latitude, double altitude)
  60 {
  61   return MakeWaypoint(Waypoint(MakeGeoPoint(longitude, latitude)), altitude);
  62 }
  63
  64 static const Waypoint wp1 = MakeWaypoint(0, 45, 50);
  65 static const Waypoint wp2 = MakeWaypoint(0, 45.3, 50);
  66 static const Waypoint wp3 = MakeWaypoint(0, 46, 50);
  67 static const Waypoint wp4 = MakeWaypoint(1, 46, 50);
  68 static const Waypoint wp5 = MakeWaypoint(0.3, 46, 50);
  69
  70 static fixed
  71 GetSafetyHeight(const TaskPoint &tp)
  72 {
  73   return task_behaviour.safety_height_arrival;
  74 }
  75
  76 static void
  77 CheckLeg(const TaskWaypoint &tp, const AircraftState &aircraft,
  78          const TaskStats &stats)
  79 {
  80   const GeoPoint destination = tp.GetWaypoint().location;
  81   const fixed safety_height = GetSafetyHeight(tp);
  82   const fixed min_arrival_alt = tp.GetWaypoint().elevation + safety_height;
  83   const GeoVector vector = aircraft.location.DistanceBearing(destination);
  84   const fixed ld = glide_polar.GetBestLD();
  85   const fixed height_above_min = aircraft.altitude - min_arrival_alt;
  86   const fixed height_consumption = vector.distance / ld;
  87   const ElementStat &leg = stats.current_leg;
  88   const GlideResult &solution_remaining = leg.solution_remaining;
  89
  90   ok1(leg.vector_remaining.IsValid());
  91   ok1(equals(leg.vector_remaining.distance, vector.distance));
  92   ok1(equals(leg.vector_remaining.bearing, vector.bearing));
  93
  94   ok1(solution_remaining.IsOk());
  95   ok1(solution_remaining.vector.IsValid());
  96   ok1(equals(solution_remaining.vector.distance, vector.distance));
  97   ok1(equals(solution_remaining.vector.bearing, vector.bearing));
  98   ok1(equals(solution_remaining.height_glide, height_consumption));
  99   ok1(equals(solution_remaining.altitude_difference,
 100              height_above_min - height_consumption));
 101   ok1(equals(solution_remaining.GetRequiredAltitudeWithDrift(),
 102              min_arrival_alt + height_consumption));
 103
 104   if (height_above_min >= height_consumption) {
 105     /* straight glide */
 106     ok1(equals(solution_remaining.height_climb, 0));
 107   } else if (positive(glide_polar.GetMC())) {
 108     /* climb required */
 109     ok1(equals(solution_remaining.height_climb,
 110                height_consumption - height_above_min));
 111   } else {
 112     /* climb required, but not possible (MC=0) */
 113     ok1(equals(solution_remaining.height_climb, 0));
 114   }
 115 }
 116
 117 static void
 118 CheckTotal(const AircraftState &aircraft, const TaskStats &stats,
 119            const TaskWaypoint &start, const TaskWaypoint &tp1,
 120            const TaskWaypoint &finish)
 121 {
 122   const fixed min_arrival_alt1 = tp1.GetWaypoint().elevation +
 123     task_behaviour.safety_height_arrival;
 124   const fixed min_arrival_alt2 = finish.GetWaypoint().elevation +
 125     task_behaviour.safety_height_arrival;
 126   const GeoVector vector0 =
 127     start.GetWaypoint().location.DistanceBearing(tp1.GetWaypoint().location);
 128   const GeoVector vector1 =
 129     aircraft.location.DistanceBearing(tp1.GetWaypoint().location);
 130   const GeoVector vector2 =
 131     tp1.GetWaypoint().location.DistanceBearing(finish.GetWaypoint().location);
 132   const fixed ld = glide_polar.GetBestLD();
 133   const fixed height_consumption1 = vector1.distance / ld;
 134
 135   const fixed height_consumption2 = vector2.distance / ld;
 136
 137   const ElementStat &total = stats.total;
 138   const GlideResult &solution_remaining = total.solution_remaining;
 139   const fixed distance_nominal = vector0.distance + vector2.distance;
 140   const fixed distance_ahead = vector1.distance + vector2.distance;
 141
 142   ok1(equals(stats.distance_nominal, distance_nominal));
 143   ok1(equals(stats.distance_min, distance_nominal));
 144   ok1(equals(stats.distance_max, distance_nominal));
 145
 146   ok1(!total.vector_remaining.IsValid());
 147   ok1(solution_remaining.IsOk());
 148
 149   ok1(equals(solution_remaining.vector.distance, distance_ahead));
 150   ok1(equals(solution_remaining.height_glide, distance_ahead / ld));
 151
 152   fixed alt_required_at_1 = std::max(min_arrival_alt1,
 153                                      min_arrival_alt2 + height_consumption2);
 154   fixed alt_required_at_aircraft = alt_required_at_1 + height_consumption1;
 155   ok1(equals(solution_remaining.GetRequiredAltitudeWithDrift(),
 156              alt_required_at_aircraft));
 157   ok1(equals(solution_remaining.altitude_difference,
 158              aircraft.altitude - alt_required_at_aircraft));
 159   ok1(equals(solution_remaining.height_climb,
 160              positive(glide_polar.GetMC())
 161              ? alt_required_at_aircraft - aircraft.altitude
 162              : fixed(0)));
 163 }
 164
 165 static void
 166 CheckLegEqualsTotal(const GlideResult &leg, const GlideResult &total)
 167 {
 168   ok1(total.IsOk());
 169   ok1(equals(total.height_climb, leg.height_climb));
 170   ok1(equals(total.height_glide, leg.height_glide));
 171   ok1(equals(total.altitude_difference, leg.altitude_difference));
 172   ok1(equals(total.GetRequiredAltitudeWithDrift(), leg.GetRequiredAltitudeWithDrift()));
 173 }
 174
 175 static void
 176 TestFlightToFinish(fixed aircraft_altitude)
 177 {
 178   OrderedTask task(task_behaviour);
 179   const StartPoint tp1(new LineSectorZone(wp1.location),
 180                        wp1, task_behaviour,
 181                        ordered_task_behaviour.start_constraints);
 182   task.Append(tp1);
 183   const FinishPoint tp2(new LineSectorZone(wp2.location),
 184                         wp2, task_behaviour,
 185                         ordered_task_behaviour.finish_constraints, false);
 186   task.Append(tp2);
 187   task.SetActiveTaskPoint(1);
 188
 189   ok1(task.CheckTask());
 190
 191   AircraftState aircraft;
 192   aircraft.Reset();
 193   aircraft.location = wp1.location;
 194   aircraft.altitude = aircraft_altitude;
 195   task.Update(aircraft, aircraft, glide_polar);
 196
 197   const GeoVector vector = wp1.location.DistanceBearing(wp2.location);
 198
 199   const TaskStats &stats = task.GetStats();
 200   ok1(stats.task_valid);
 201   ok1(!stats.task_started);
 202   ok1(!stats.task_finished);
 203   ok1(stats.flight_mode_final_glide == !negative(stats.total.solution_remaining.altitude_difference));
 204   ok1(equals(stats.distance_nominal, vector.distance));
 205   ok1(equals(stats.distance_min, vector.distance));
 206   ok1(equals(stats.distance_max, vector.distance));
 207
 208   CheckLeg(tp2, aircraft, stats);
 209
 210   ok1(!stats.total.vector_remaining.IsValid());
 211   CheckLegEqualsTotal(stats.current_leg.solution_remaining,
 212                       stats.total.solution_remaining);
 213 }
 214
 215 static void
 216 TestSimpleTask()
 217 {
 218   OrderedTask task(task_behaviour);
 219   const StartPoint tp1(new LineSectorZone(wp1.location),
 220                        wp1, task_behaviour,
 221                        ordered_task_behaviour.start_constraints);
 222   task.Append(tp1);
 223   const FinishPoint tp2(new LineSectorZone(wp3.location),
 224                         wp3, task_behaviour,
 225                         ordered_task_behaviour.finish_constraints, false);
 226   task.Append(tp2);
 227
 228   ok1(task.CheckTask());
 229
 230   AircraftState aircraft;
 231   aircraft.Reset();
 232   aircraft.location = MakeGeoPoint(0, 44.5);
 233   aircraft.altitude = fixed(1700);
 234   task.Update(aircraft, aircraft, glide_polar);
 235
 236   const GeoVector tp1_to_tp2 = wp1.location.DistanceBearing(wp3.location);
 237
 238   const TaskStats &stats = task.GetStats();
 239   ok1(stats.task_valid);
 240   ok1(!stats.task_started);
 241   ok1(!stats.task_finished);
 242   ok1(!stats.flight_mode_final_glide);
 243   ok1(equals(stats.distance_nominal, tp1_to_tp2.distance));
 244   ok1(equals(stats.distance_min, tp1_to_tp2.distance));
 245   ok1(equals(stats.distance_max, tp1_to_tp2.distance));
 246
 247   CheckLeg(tp1, aircraft, stats);
 248   CheckTotal(aircraft, stats, tp1, tp1, tp2);
 249 }
 250
 251 static void
 252 TestHighFinish()
 253 {
 254   OrderedTask task(task_behaviour);
 255   const StartPoint tp1(new LineSectorZone(wp1.location),
 256                        wp1, task_behaviour,
 257                        ordered_task_behaviour.start_constraints);
 258   task.Append(tp1);
 259   Waypoint wp2b(wp2);
 260   wp2b.elevation = fixed(1000);
 261   const FinishPoint tp2(new LineSectorZone(wp2b.location),
 262                         wp2b, task_behaviour,
 263                         ordered_task_behaviour.finish_constraints, false);
 264   task.Append(tp2);
 265   task.SetActiveTaskPoint(1);
 266
 267   ok1(task.CheckTask());
 268
 269   AircraftState aircraft;
 270   aircraft.Reset();
 271   aircraft.location = wp1.location;
 272   aircraft.altitude = fixed(1000);
 273   task.Update(aircraft, aircraft, glide_polar);
 274
 275   const GeoVector vector = wp1.location.DistanceBearing(wp2.location);
 276
 277   const TaskStats &stats = task.GetStats();
 278   ok1(stats.task_valid);
 279   ok1(!stats.task_started);
 280   ok1(!stats.task_finished);
 281   ok1(!stats.flight_mode_final_glide);
 282   ok1(equals(stats.distance_nominal, vector.distance));
 283   ok1(equals(stats.distance_min, vector.distance));
 284   ok1(equals(stats.distance_max, vector.distance));
 285
 286   CheckLeg(tp2, aircraft, stats);
 287
 288   ok1(!stats.total.vector_remaining.IsValid());
 289   CheckLegEqualsTotal(stats.current_leg.solution_remaining,
 290                       stats.total.solution_remaining);
 291 }
 292
 293 static void
 294 TestHighTP()
 295 {
 296   const fixed width(1);
 297   OrderedTask task(task_behaviour);
 298   const StartPoint tp1(new LineSectorZone(wp1.location, width),
 299                        wp1, task_behaviour,
 300                        ordered_task_behaviour.start_constraints);
 301   task.Append(tp1);
 302   const Waypoint wp3b = MakeWaypoint(wp3, 1500);
 303   const ASTPoint tp2(new LineSectorZone(wp3b.location, width),
 304                      wp3b, task_behaviour);
 305   task.Append(tp2);
 306   const Waypoint wp4b = MakeWaypoint(wp4, 100);
 307   const FinishPoint tp3(new LineSectorZone(wp4b.location, width),
 308                         wp4b, task_behaviour,
 309                         ordered_task_behaviour.finish_constraints, false);
 310   task.Append(tp3);
 311   task.SetActiveTaskPoint(1);
 312
 313   ok1(task.CheckTask());
 314
 315   AircraftState aircraft;
 316   aircraft.Reset();
 317   aircraft.location = wp1.location;
 318   aircraft.altitude = fixed(2000);
 319   task.Update(aircraft, aircraft, glide_polar);
 320
 321   const TaskStats &stats = task.GetStats();
 322   ok1(stats.task_valid);
 323   ok1(!stats.task_started);
 324   ok1(!stats.task_finished);
 325   ok1(!stats.flight_mode_final_glide);
 326
 327   CheckLeg(tp2, aircraft, stats);
 328   CheckTotal(aircraft, stats, tp1, tp2, tp3);
 329 }
 330
 331 static void
 332 TestHighTPFinal()
 333 {
 334   const fixed width(1);
 335   OrderedTask task(task_behaviour);
 336   const StartPoint tp1(new LineSectorZone(wp1.location, width),
 337                        wp1, task_behaviour,
 338                        ordered_task_behaviour.start_constraints);
 339   task.Append(tp1);
 340   const Waypoint wp3b = MakeWaypoint(wp3, 1500);
 341   const ASTPoint tp2(new LineSectorZone(wp3b.location, width),
 342                      wp3b, task_behaviour);
 343   task.Append(tp2);
 344   const Waypoint wp5b = MakeWaypoint(wp5, 200);
 345   const FinishPoint tp3(new LineSectorZone(wp5b.location, width),
 346                         wp5b, task_behaviour,
 347                         ordered_task_behaviour.finish_constraints, false);
 348   task.Append(tp3);
 349   task.SetActiveTaskPoint(1);
 350
 351   ok1(task.CheckTask());
 352
 353   AircraftState aircraft;
 354   aircraft.Reset();
 355   aircraft.location = wp1.location;
 356   aircraft.altitude = fixed(1200);
 357   task.Update(aircraft, aircraft, glide_polar);
 358
 359   const TaskStats &stats = task.GetStats();
 360   ok1(stats.task_valid);
 361   ok1(!stats.task_started);
 362   ok1(!stats.task_finished);
 363   ok1(!stats.flight_mode_final_glide);
 364
 365   CheckLeg(tp2, aircraft, stats);
 366   CheckTotal(aircraft, stats, tp1, tp2, tp3);
 367 }
 368
 369 static void
 370 TestLowTPFinal()
 371 {
 372   const fixed width(1);
 373   OrderedTask task(task_behaviour);
 374   const Waypoint wp1b = MakeWaypoint(wp1, 1500);
 375   const StartPoint tp1(new LineSectorZone(wp1b.location, width),
 376                        wp1b, task_behaviour,
 377                        ordered_task_behaviour.start_constraints);
 378   task.Append(tp1);
 379   const ASTPoint tp2(new LineSectorZone(wp2.location, width),
 380                      wp2, task_behaviour);
 381   task.Append(tp2);
 382   const FinishPoint tp3(new LineSectorZone(wp3.location, width),
 383                         wp3, task_behaviour,
 384                         ordered_task_behaviour.finish_constraints, false);
 385   task.Append(tp3);
 386   task.SetActiveTaskPoint(1);
 387
 388   ok1(task.CheckTask());
 389
 390   AircraftState aircraft;
 391   aircraft.Reset();
 392   aircraft.location = wp1.location;
 393   aircraft.altitude = fixed(2500);
 394   task.Update(aircraft, aircraft, glide_polar);
 395
 396   const TaskStats &stats = task.GetStats();
 397   ok1(stats.task_valid);
 398   ok1(!stats.task_started);
 399   ok1(!stats.task_finished);
 400   ok1(!stats.flight_mode_final_glide);
 401
 402   CheckLeg(tp2, aircraft, stats);
 403   CheckTotal(aircraft, stats, tp1, tp2, tp3);
 404 }
 405
 406 static void
 407 TestAll()
 408 {
 409   TestFlightToFinish(fixed(2000));
 410   TestFlightToFinish(fixed(1000));
 411   TestSimpleTask();
 412   TestHighFinish();
 413   TestHighTP();
 414   TestHighTPFinal();
 415   TestLowTPFinal();
 416 }
 417
 418 int main(int argc, char **argv)
 419 {
 420   plan_tests(728);
 421
 422   task_behaviour.SetDefaults();
 423
 424   TestAll();
 425
 426   glide_polar.SetMC(fixed(1));
 427   TestAll();
 428
 429   glide_polar.SetMC(fixed(2));
 430   TestAll();
 431
 432   glide_polar.SetMC(fixed(4));
 433   TestAll();
 434
 435   return exit_status();
 436 }