src/Computer/CirclingComputer.cpp

   1 /*
   2 Copyright_License {
   3
   4   XCSoar Glide Computer - http://www.xcsoar.org/
   5   Copyright (C) 2000-2013 The XCSoar Project
   6   A detailed list of copyright holders can be found in the file "AUTHORS".
   7
   8   This program is free software; you can redistribute it and/or
   9   modify it under the terms of the GNU General Public License
  10   as published by the Free Software Foundation; either version 2
  11   of the License, or (at your option) any later version.
  12
  13   This program is distributed in the hope that it will be useful,
  14   but WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16   GNU General Public License for more details.
  17
  18   You should have received a copy of the GNU General Public License
  19   along with this program; if not, write to the Free Software
  20   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  21 }
  22 */
  23
  24 #include "CirclingComputer.hpp"
  25 #include "NMEA/MoreData.hpp"
  26 #include "NMEA/Derived.hpp"
  27 #include "Settings.hpp"
  28 #include "Math/LowPassFilter.hpp"
  29 #include "Util/Clamp.hpp"
  30
  31 static constexpr fixed MIN_TURN_RATE(4);
  32 static constexpr fixed CRUISE_CLIMB_SWITCH(15);
  33 static constexpr fixed CLIMB_CRUISE_SWITCH(10);
  34
  35 void
  36 CirclingComputer::Reset()
  37 {
  38   turn_rate_delta_time.Reset();
  39   turning_delta_time.Reset();
  40   percent_delta_time.Reset();
  41
  42   ResetStats();
  43 }
  44
  45 void
  46 CirclingComputer::ResetStats()
  47 {
  48   min_altitude = fixed(0);
  49 }
  50
  51 void
  52 CirclingComputer::TurnRate(CirclingInfo &circling_info,
  53                            const NMEAInfo &basic,
  54                            const FlyingState &flight)
  55 {
  56   if (!basic.time_available || !flight.flying) {
  57     circling_info.turn_rate = fixed(0);
  58     circling_info.turn_rate_heading = fixed(0);
  59     circling_info.turn_rate_smoothed = fixed(0);
  60     return;
  61   }
  62
  63   const fixed dt = turn_rate_delta_time.Update(basic.time,
  64                                                fixed_third, fixed(10));
  65   if (negative(dt)) {
  66     circling_info.turn_rate = fixed(0);
  67     circling_info.turn_rate_heading = fixed(0);
  68     circling_info.turn_rate_smoothed = fixed(0);
  69     return;
  70   }
  71
  72   if (positive(dt)) {
  73     circling_info.turn_rate =
  74       (basic.track - last_track).AsDelta().Degrees() / dt;
  75     circling_info.turn_rate_heading =
  76       (basic.attitude.heading - last_heading).AsDelta().Degrees() / dt;
  77
  78     // JMW limit rate to 50 deg per second otherwise a big spike
  79     // will cause spurious lock on circling for a long time
  80     fixed turn_rate = Clamp(circling_info.turn_rate, fixed(-50), fixed(50));
  81
  82     // Make the turn rate more smooth using the LowPassFilter
  83     turn_rate = LowPassFilter(circling_info.turn_rate_smoothed,
  84                               turn_rate, fixed(0.3));
  85     circling_info.turn_rate_smoothed = turn_rate;
  86   }
  87
  88   last_track = basic.track;
  89   last_heading = basic.attitude.heading;
  90 }
  91
  92 void
  93 CirclingComputer::Turning(CirclingInfo &circling_info,
  94                           const MoreData &basic,
  95                           const FlyingState &flight,
  96                           const CirclingSettings &settings)
  97 {
  98   // You can't be circling unless you're flying
  99   if (!basic.time_available || !flight.flying)
 100     return;
 101
 102   const fixed dt = turning_delta_time.Update(basic.time,
 103                                              fixed(0), fixed(0));
 104   if (!positive(dt))
 105     return;
 106
 107   circling_info.turning =
 108     fabs(circling_info.turn_rate_smoothed) >= MIN_TURN_RATE;
 109
 110   // Force cruise or climb mode if external device says so
 111   bool force_cruise = false;
 112   bool force_circling = false;
 113   if (settings.external_trigger_cruise_enabled && !basic.gps.replay) {
 114     switch (basic.switch_state.flight_mode) {
 115     case SwitchState::FlightMode::UNKNOWN:
 116       break;
 117
 118     case SwitchState::FlightMode::CIRCLING:
 119       force_circling = true;
 120       break;
 121
 122     case SwitchState::FlightMode::CRUISE:
 123       force_cruise = true;
 124       break;
 125     }
 126   }
 127
 128   switch (circling_info.turn_mode) {
 129   case CirclingMode::CRUISE:
 130     // If (in cruise mode and beginning of circling detected)
 131     if (circling_info.turning || force_circling) {
 132       // Remember the start values of the turn
 133       turn_start_time = basic.time;
 134       turn_start_location = basic.location;
 135       turn_start_altitude = basic.nav_altitude;
 136       turn_start_energy_height = basic.energy_height;
 137       circling_info.turn_mode = CirclingMode::POSSIBLE_CLIMB;
 138     }
 139     if (!force_circling)
 140       break;
 141
 142   case CirclingMode::POSSIBLE_CLIMB:
 143     if (force_cruise) {
 144       circling_info.turn_mode = CirclingMode::CRUISE;
 145       break;
 146     }
 147     if (circling_info.turning || force_circling) {
 148       if (((basic.time - turn_start_time) > CRUISE_CLIMB_SWITCH)
 149           || force_circling) {
 150         // yes, we are certain now that we are circling
 151         circling_info.circling = true;
 152
 153         // JMW Transition to climb
 154         circling_info.turn_mode = CirclingMode::CLIMB;
 155
 156         // Remember the start values of the climbing period
 157         circling_info.climb_start_location = turn_start_location;
 158         circling_info.climb_start_altitude = turn_start_altitude;
 159         circling_info.climb_start_altitude_te =
 160           turn_start_altitude + turn_start_energy_height;
 161         circling_info.climb_start_time = turn_start_time;
 162       }
 163     } else {
 164       // nope, not turning, so go back to cruise
 165       circling_info.turn_mode = CirclingMode::CRUISE;
 166     }
 167     break;
 168
 169   case CirclingMode::CLIMB:
 170     if (!circling_info.turning || force_cruise) {
 171       // Remember the end values of the turn
 172       turn_start_time = basic.time;
 173       turn_start_location = basic.location;
 174       turn_start_altitude = basic.nav_altitude;
 175       turn_start_energy_height = basic.energy_height;
 176
 177       // JMW Transition to cruise, due to not properly turning
 178       circling_info.turn_mode = CirclingMode::POSSIBLE_CRUISE;
 179     }
 180     if (!force_cruise)
 181       break;
 182
 183   case CirclingMode::POSSIBLE_CRUISE:
 184     if (force_circling) {
 185       circling_info.turn_mode = CirclingMode::CLIMB;
 186       break;
 187     }
 188
 189     if (!circling_info.turning || force_cruise) {
 190       if (basic.time - turn_start_time > CLIMB_CRUISE_SWITCH || force_cruise) {
 191         // yes, we are certain now that we are cruising again
 192         circling_info.circling = false;
 193
 194         // Transition to cruise
 195         circling_info.turn_mode = CirclingMode::CRUISE;
 196         circling_info.cruise_start_location = turn_start_location;
 197         circling_info.cruise_start_altitude = turn_start_altitude;
 198         circling_info.cruise_start_altitude_te =
 199           turn_start_altitude + turn_start_energy_height;
 200         circling_info.cruise_start_time = turn_start_time;
 201       }
 202     } else {
 203       // nope, we are circling again
 204       // JMW Transition back to climb, because we are turning again
 205       circling_info.turn_mode = CirclingMode::CLIMB;
 206     }
 207     break;
 208   }
 209 }
 210
 211 void
 212 CirclingComputer::PercentCircling(const MoreData &basic,
 213                                   CirclingInfo &circling_info)
 214 {
 215   if (!basic.time_available)
 216     return;
 217
 218   // JMW circling % only when really circling,
 219   // to prevent bad stats due to flap switches and dolphin soaring
 220
 221   const fixed dt = percent_delta_time.Update(basic.time,
 222                                              fixed(0), fixed(0));
 223   if (!positive(dt))
 224     return;
 225
 226   // if (Circling)
 227   if (circling_info.circling && circling_info.turning) {
 228     // Add one second to the circling time
 229     // timeCircling += (Basic->Time-LastTime);
 230     circling_info.time_climb += dt;
 231
 232     // Add the Vario signal to the total climb height
 233     circling_info.total_height_gain += basic.gps_vario;
 234   } else {
 235     // Add one second to the cruise time
 236     // timeCruising += (Basic->Time-LastTime);
 237     circling_info.time_cruise += dt;
 238   }
 239
 240   // Calculate the circling percentage
 241   if (circling_info.time_cruise + circling_info.time_climb > fixed(1))
 242     circling_info.circling_percentage = 100 * circling_info.time_climb /
 243         (circling_info.time_cruise + circling_info.time_climb);
 244   else
 245     circling_info.circling_percentage = fixed(-1);
 246 }
 247
 248 void
 249 CirclingComputer::MaxHeightGain(const MoreData &basic,
 250                                 const FlyingState &flight,
 251                                 CirclingInfo &circling_info)
 252 {
 253   if (!basic.NavAltitudeAvailable() || !flight.flying)
 254     return;
 255
 256   if (positive(min_altitude)) {
 257     fixed height_gain = basic.nav_altitude - min_altitude;
 258     circling_info.max_height_gain =
 259       std::max(height_gain, circling_info.max_height_gain);
 260   } else {
 261     min_altitude = basic.nav_altitude;
 262   }
 263
 264   min_altitude = std::min(basic.nav_altitude, min_altitude);
 265 }