test/src/FlightPhaseDetector.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 "FlightPhaseDetector.hpp"
  24 #include "NMEA/CirclingInfo.hpp"
  25 #include "NMEA/MoreData.hpp"
  26 #include "NMEA/Derived.hpp"
  27
  28 constexpr fixed MIN_PHASE_TIME = fixed(30);
  29 constexpr fixed FP_TOLERANCE = fixed(0.001);
  30
  31 /**
  32  * Update circling directon for given phase with given new direction
  33  *
  34  * If direction change is detected, phase direction will become MIXED.
  35  * Note, that direction will be set for circling phases only.
  36  */
  37 static void
  38 UpdateCirclingDirection(Phase &phase, const Phase::CirclingDirection dir)
  39 {
  40   if (phase.phase_type != Phase::Type::CIRCLING) {
  41     phase.circling_direction = Phase::CirclingDirection::NO_DIRECTION;
  42     return;
  43   }
  44
  45   if (dir == Phase::CirclingDirection::NO_DIRECTION) {
  46     return;
  47   }
  48   if (phase.circling_direction != dir) {
  49     if (phase.circling_direction == Phase::CirclingDirection::NO_DIRECTION) {
  50       phase.circling_direction = dir;
  51     }
  52     else {
  53       phase.circling_direction = Phase::CirclingDirection::MIXED;
  54     }
  55   }
  56 }
  57
  58 static void
  59 CombinePhases(Phase &base, const Phase &addition)
  60 {
  61   UpdateCirclingDirection(base, addition.circling_direction);
  62   base.end_datetime = addition.end_datetime;
  63   base.end_time = addition.end_time;
  64   base.end_alt = addition.end_alt;
  65   base.end_loc = addition.end_loc;
  66   base.distance += addition.distance;
  67   base.duration += addition.duration;
  68   base.alt_diff += addition.alt_diff;
  69   base.merges++;
  70 }
  71
  72 static Phase::Type
  73 GetPhaseType(const DerivedInfo &calculated)
  74 {
  75   switch (calculated.turn_mode) {
  76   case CirclingMode::CRUISE:
  77     return Phase::Type::CRUISE;
  78   case CirclingMode::CLIMB:
  79     return Phase::Type::CIRCLING;
  80   default:
  81     return Phase::Type::NO_PHASE;
  82   }
  83 }
  84
  85 /**
  86  * Return true if current flight is appears to be powered, false otherwise
  87  */
  88 static bool
  89 IsPoweredFlight(const DerivedInfo &calculated)
  90 {
  91   return calculated.flight.IsTowing();
  92 }
  93
  94 static Phase::CirclingDirection
  95 CalcCirclingDirection(const DerivedInfo &calculated)
  96 {
  97   if (!calculated.circling) {
  98     return Phase::CirclingDirection::NO_DIRECTION;
  99   }
 100   return calculated.TurningLeft() ?
 101          Phase::CirclingDirection::LEFT : Phase::CirclingDirection::RIGHT;
 102 }
 103
 104 fixed
 105 Phase::GetSpeed() const {
 106   if (duration < FP_TOLERANCE) {
 107     return fixed(0);
 108   }
 109   return distance / duration;
 110 }
 111
 112 fixed
 113 Phase::GetVario() const {
 114   if (duration < FP_TOLERANCE) {
 115     return fixed(0);
 116   }
 117   return alt_diff / duration;
 118 }
 119
 120 fixed
 121 Phase::GetGlideRate() const {
 122   if (fabs(alt_diff) < FP_TOLERANCE) {
 123     return fixed(0);
 124   }
 125   return distance / -alt_diff;
 126 }
 127
 128 FlightPhaseDetector::FlightPhaseDetector()
 129 {
 130   previous_phase.Clear();
 131   current_phase.Clear();
 132   phase_count = 0;
 133 }
 134
 135 void
 136 FlightPhaseDetector::Update(const MoreData &basic, const DerivedInfo &calculated)
 137 {
 138   bool mode_changed = last_turn_mode != calculated.turn_mode;
 139   bool lost_power = current_phase.phase_type == Phase::Type::POWERED &&
 140                     !IsPoweredFlight(calculated);
 141
 142   bool phase_changed = mode_changed || lost_power;
 143
 144   if (phase_changed) {
 145     if (current_phase.phase_type != Phase::Type::NO_PHASE) {
 146       PushPhase();
 147     }
 148     else {
 149       /* uncertain turn mode, try to determine it now and continue filling this
 150        * phase. */
 151       current_phase.phase_type = GetPhaseType(calculated);
 152     }
 153   }
 154
 155   if (!current_phase.start_datetime.Plausible()) {
 156     // Initialize new phase.
 157     current_phase.phase_type = GetPhaseType(calculated);
 158     current_phase.end_loc = basic.location;
 159     /* First phase' start_attributes are initialized with factual data,
 160      * subsequent phases starts where previous one ended. */
 161     if (phase_count == 0) {
 162       current_phase.start_datetime = basic.date_time_utc;
 163       current_phase.start_time = basic.time;
 164       current_phase.start_loc = basic.location;
 165       current_phase.start_alt = basic.nav_altitude;
 166     }
 167     else {
 168       current_phase.start_datetime = previous_phase.end_datetime;
 169       current_phase.start_time = previous_phase.end_time;
 170       current_phase.start_loc = previous_phase.end_loc;
 171       current_phase.start_alt = previous_phase.end_alt;
 172     }
 173   }
 174   // Update the current phase with additional data
 175   current_phase.duration = basic.time - current_phase.start_time;
 176   current_phase.alt_diff = basic.nav_altitude - current_phase.start_alt;
 177   current_phase.distance += current_phase.end_loc.Distance(basic.location);
 178   current_phase.end_datetime = basic.date_time_utc;
 179   current_phase.end_time = basic.time;
 180   current_phase.end_loc = basic.location;
 181   current_phase.end_alt = basic.nav_altitude;
 182   UpdateCirclingDirection(current_phase, CalcCirclingDirection(calculated));
 183   // When powered flight is detected, current phase becomes "powered"
 184   if (IsPoweredFlight(calculated)) {
 185     current_phase.phase_type = Phase::Type::POWERED;
 186   }
 187
 188   // detect powered flight
 189
 190   // save last turn mode seen
 191   last_turn_mode = calculated.turn_mode;
 192 }
 193
 194 void
 195 FlightPhaseDetector::Finish()
 196 {
 197   // Push last phases (we still have two latest phases in state)
 198   if (current_phase.phase_type == Phase::Type::NO_PHASE) {
 199     current_phase.phase_type = previous_phase.phase_type;
 200   }
 201   PushPhase();
 202   phases.push_back(previous_phase);
 203
 204   // Calculate total statistics
 205   for (Phase ph : phases) {
 206     switch (ph.phase_type) {
 207     case Phase::Type::CIRCLING:
 208       CombinePhases(totals.total_circstats, ph);
 209       switch (ph.circling_direction) {
 210       case Phase::CirclingDirection::LEFT:
 211         CombinePhases(totals.left_circstats, ph);
 212         break;
 213       case Phase::CirclingDirection::RIGHT:
 214         CombinePhases(totals.right_circstats, ph);
 215         break;
 216       case Phase::CirclingDirection::MIXED:
 217         CombinePhases(totals.mixed_circstats, ph);
 218         break;
 219       case Phase::CirclingDirection::NO_DIRECTION:
 220         break;
 221       }
 222       break;
 223     case Phase::Type::CRUISE:
 224       CombinePhases(totals.total_cruisestats, ph);
 225       break;
 226     case Phase::Type::POWERED:
 227     case Phase::Type::NO_PHASE:
 228       break;
 229     }
 230   }
 231
 232   // Calculate fractions
 233   fixed total_circling = totals.total_circstats.duration;
 234   fixed total_cruise = totals.total_cruisestats.duration;
 235   fixed total_duration = total_circling + total_cruise;
 236   if (total_duration > fixed(0)) {
 237       totals.total_circstats.fraction = total_circling / total_duration;
 238       totals.total_cruisestats.fraction = total_cruise / total_duration;
 239
 240       if (total_circling > fixed(0)) {
 241           totals.left_circstats.fraction =
 242               totals.left_circstats.duration / total_circling;
 243           totals.right_circstats.fraction =
 244               totals.right_circstats.duration / total_circling;
 245           totals.mixed_circstats.fraction =
 246               totals.mixed_circstats.duration / total_circling;
 247       }
 248   }
 249 }
 250
 251 void
 252 FlightPhaseDetector::PushPhase()
 253 {
 254   assert(current_phase.phase_type != Phase::Type::NO_PHASE);
 255
 256   if (phase_count == 0) {
 257     previous_phase = current_phase;
 258     phase_count++;
 259   }
 260   else {
 261     bool same_type = previous_phase.phase_type == current_phase.phase_type;
 262     bool too_short = current_phase.duration < MIN_PHASE_TIME;
 263     if (same_type || too_short) {
 264       CombinePhases(previous_phase, current_phase);
 265     }
 266     else {
 267       phases.push_back(previous_phase);
 268       phase_count++;
 269       previous_phase = current_phase;
 270     }
 271   }
 272   current_phase.Clear();
 273 }