src/Computer/GlideComputerAirData.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
  25 #include "GlideComputerAirData.hpp"
  26 #include "GlideComputer.hpp"
  27 #include "Settings.hpp"
  28 #include "Math/LowPassFilter.hpp"
  29 #include "Terrain/RasterTerrain.hpp"
  30 #include "ThermalBase.hpp"
  31 #include "GlideSolvers/GlidePolar.hpp"
  32 #include "NMEA/Aircraft.hpp"
  33 #include "Math/SunEphemeris.hpp"
  34
  35 static constexpr fixed THERMAL_TIME_MIN(45);
  36
  37 GlideComputerAirData::GlideComputerAirData(const Waypoints &_way_points)
  38   :waypoints(_way_points),
  39    terrain(NULL)
  40 {
  41   // JMW TODO enhancement: seed initial wind store with start conditions
  42   // SetWindEstimate(Calculated().WindSpeed, Calculated().WindBearing, 1);
  43 }
  44
  45 void
  46 GlideComputerAirData::ResetFlight(DerivedInfo &calculated,
  47                                   const bool full)
  48 {
  49   auto_qnh.Reset();
  50
  51   average_vario.Reset();
  52
  53   lift_database_computer.Reset(calculated.lift_database,
  54                                calculated.trace_history.CirclingAverage);
  55
  56   thermallocator.Reset();
  57
  58   gr_computer.Reset();
  59
  60   if (full)
  61     flying_computer.Reset();
  62
  63   circling_computer.Reset();
  64
  65   thermal_band_computer.Reset();
  66   wind_computer.Reset();
  67 }
  68
  69 void
  70 GlideComputerAirData::ProcessBasic(const MoreData &basic,
  71                                    DerivedInfo &calculated,
  72                                    const ComputerSettings &settings)
  73 {
  74   TerrainHeight(basic, calculated);
  75   ProcessSun(basic, calculated, settings);
  76
  77   NettoVario(basic, calculated.flight, calculated, settings);
  78 }
  79
  80 void
  81 GlideComputerAirData::ProcessVertical(const MoreData &basic,
  82                                       const MoreData &last_basic,
  83                                       DerivedInfo &calculated,
  84                                       const ComputerSettings &settings)
  85 {
  86   /* the "circling" flag may be modified by
  87      CirclingComputer::Turning(); remember the old state so this
  88      method can check for modifications */
  89   const bool last_circling = calculated.circling;
  90
  91   auto_qnh.Process(basic, calculated, settings, waypoints);
  92
  93   circling_computer.TurnRate(calculated, basic,
  94                              calculated.flight);
  95   Turning(basic, calculated, settings);
  96
  97   wind_computer.Compute(settings.wind, settings.polar.glide_polar_task,
  98                         basic, calculated);
  99   wind_computer.Select(settings.wind, basic, calculated);
 100   wind_computer.ComputeHeadWind(basic, calculated);
 101
 102   thermallocator.Process(calculated.circling,
 103                          basic.time, basic.location,
 104                          basic.netto_vario,
 105                          calculated.GetWindOrZero(),
 106                          calculated.thermal_locator);
 107
 108   LastThermalStats(basic, calculated, last_circling);
 109
 110   gr_computer.Compute(basic, last_basic, calculated,
 111                       calculated,
 112                       settings);
 113
 114   GR(basic, calculated.flight, calculated);
 115   CruiseGR(basic, calculated);
 116
 117   average_vario.Compute(basic, calculated.circling, last_circling,
 118                         calculated);
 119   AverageClimbRate(basic, calculated);
 120
 121   if (calculated.circling)
 122     CurrentThermal(basic, calculated, calculated.current_thermal);
 123
 124   lift_database_computer.Compute(calculated.lift_database,
 125                                  calculated.trace_history.CirclingAverage,
 126                                  basic, calculated);
 127   circling_computer.MaxHeightGain(basic, calculated.flight, calculated);
 128   NextLegEqThermal(basic, calculated, settings);
 129 }
 130
 131 inline void
 132 GlideComputerAirData::NettoVario(const NMEAInfo &basic,
 133                                  const FlyingState &flight,
 134                                  VarioInfo &vario,
 135                                  const ComputerSettings &settings_computer)
 136 {
 137   fixed g_load = basic.acceleration.available ?
 138                  basic.acceleration.g_load : fixed(1);
 139
 140   vario.sink_rate =
 141     flight.flying && basic.airspeed_available &&
 142     settings_computer.polar.glide_polar_task.IsValid()
 143     ? - settings_computer.polar.glide_polar_task.SinkRate(basic.indicated_airspeed,
 144                                                           g_load)
 145     /* the glider sink rate is useless when not flying */
 146     : fixed(0);
 147 }
 148
 149 inline void
 150 GlideComputerAirData::AverageClimbRate(const NMEAInfo &basic,
 151                                        DerivedInfo &calculated)
 152 {
 153   if (basic.airspeed_available && positive(basic.indicated_airspeed) &&
 154       positive(basic.true_airspeed) &&
 155       basic.total_energy_vario_available &&
 156       !calculated.circling &&
 157       (!basic.acceleration.available ||
 158        !basic.acceleration.real ||
 159        fabs(basic.acceleration.g_load - fixed(1)) <= fixed(0.25))) {
 160     // TODO: Check this is correct for TAS/IAS
 161     fixed ias_to_tas = basic.indicated_airspeed / basic.true_airspeed;
 162     fixed w_tas = basic.total_energy_vario * ias_to_tas;
 163
 164     calculated.climb_history.Add(uround(basic.indicated_airspeed), w_tas);
 165   }
 166 }
 167
 168 inline void
 169 GlideComputerAirData::CurrentThermal(const MoreData &basic,
 170                                      const CirclingInfo &circling,
 171                                      OneClimbInfo &current_thermal)
 172 {
 173   if (positive(circling.climb_start_time)) {
 174     current_thermal.start_time = circling.climb_start_time;
 175     current_thermal.end_time = basic.time;
 176     current_thermal.gain =
 177       basic.TE_altitude - circling.climb_start_altitude_te;
 178     current_thermal.CalculateAll();
 179   } else
 180     current_thermal.Clear();
 181 }
 182
 183 inline void
 184 GlideComputerAirData::GR(const MoreData &basic, const FlyingState &flying,
 185                          VarioInfo &vario_info)
 186 {
 187   // Lift / drag instantaneous from vario, updated every reading..
 188   if (basic.total_energy_vario_available && basic.airspeed_available &&
 189       flying.flying) {
 190     vario_info.ld_vario =
 191       UpdateGR(vario_info.ld_vario, basic.indicated_airspeed,
 192                -basic.total_energy_vario, fixed(0.3));
 193   } else {
 194     vario_info.ld_vario = INVALID_GR;
 195   }
 196 }
 197
 198 inline void
 199 GlideComputerAirData::CruiseGR(const MoreData &basic, DerivedInfo &calculated)
 200 {
 201   if (!calculated.circling && basic.NavAltitudeAvailable()) {
 202     if (negative(calculated.cruise_start_time)) {
 203       calculated.cruise_start_location = basic.location;
 204       calculated.cruise_start_altitude = basic.nav_altitude;
 205       calculated.cruise_start_time = basic.time;
 206     } else {
 207       fixed DistanceFlown =
 208         basic.location.Distance(calculated.cruise_start_location);
 209
 210       calculated.cruise_gr =
 211           UpdateGR(calculated.cruise_gr, DistanceFlown,
 212                    calculated.cruise_start_altitude - basic.nav_altitude,
 213                    fixed(0.5));
 214     }
 215   }
 216 }
 217
 218 /**
 219  * Reads the current terrain height
 220  */
 221 inline void
 222 GlideComputerAirData::TerrainHeight(const MoreData &basic,
 223                                     TerrainInfo &calculated)
 224 {
 225   if (!basic.location_available || terrain == NULL) {
 226     calculated.terrain_valid = false;
 227     calculated.terrain_altitude = fixed(0);
 228     calculated.altitude_agl_valid = false;
 229     calculated.altitude_agl = fixed(0);
 230     return;
 231   }
 232
 233   short Alt = terrain->GetTerrainHeight(basic.location);
 234   if (RasterBuffer::IsSpecial(Alt)) {
 235     if (RasterBuffer::IsWater(Alt))
 236       /* assume water is 0m MSL; that's the best guess */
 237       Alt = 0;
 238     else {
 239       calculated.terrain_valid = false;
 240       calculated.terrain_altitude = fixed(0);
 241       calculated.altitude_agl_valid = false;
 242       calculated.altitude_agl = fixed(0);
 243       return;
 244     }
 245   }
 246
 247   calculated.terrain_valid = true;
 248   calculated.terrain_altitude = fixed(Alt);
 249
 250   if (basic.NavAltitudeAvailable()) {
 251     calculated.altitude_agl = basic.nav_altitude - calculated.terrain_altitude;
 252     calculated.altitude_agl_valid = true;
 253   } else
 254     calculated.altitude_agl_valid = false;
 255 }
 256
 257 bool
 258 GlideComputerAirData::FlightTimes(const NMEAInfo &basic,
 259                                   const NMEAInfo &last_basic,
 260                                   DerivedInfo &calculated,
 261                                   const ComputerSettings &settings)
 262 {
 263   if (basic.gps.replay != last_basic.gps.replay)
 264     // reset flight before/after replay logger
 265     ResetFlight(calculated, basic.gps.replay);
 266
 267   if (basic.time_available && basic.HasTimeRetreatedSince(last_basic)) {
 268     // 20060519:sgi added (basic.Time != 0) due to always return here
 269     // if no GPS time available
 270     if (basic.location_available)
 271       // Reset statistics.. (probably due to being in IGC replay mode)
 272       ResetFlight(calculated, false);
 273
 274     return false;
 275   }
 276
 277   FlightState(basic, calculated, calculated.flight,
 278               settings.polar.glide_polar_task);
 279
 280   return true;
 281 }
 282
 283 inline void
 284 GlideComputerAirData::FlightState(const NMEAInfo &basic,
 285                                   const DerivedInfo &calculated,
 286                                   FlyingState &flying,
 287                                   const GlidePolar &glide_polar)
 288 {
 289   fixed v_takeoff = glide_polar.IsValid()
 290     ? glide_polar.GetVTakeoff()
 291     /* if there's no valid polar, assume 10 m/s (36 km/h); that's an
 292        arbitrary value, but better than nothing */
 293     : fixed(10);
 294
 295   flying_computer.Compute(v_takeoff, basic,
 296                           calculated, flying);
 297 }
 298
 299 inline void
 300 GlideComputerAirData::Turning(const MoreData &basic,
 301                               DerivedInfo &calculated,
 302                               const ComputerSettings &settings)
 303 {
 304   circling_computer.Turning(calculated,
 305                             basic,
 306                             calculated.flight,
 307                             settings.circling);
 308
 309   // Calculate circling time percentage and call thermal band calculation
 310   circling_computer.PercentCircling(basic, calculated);
 311
 312   thermal_band_computer.Compute(basic, calculated,
 313                                 calculated.thermal_band,
 314                                 settings);
 315 }
 316
 317 inline void
 318 GlideComputerAirData::ThermalSources(const MoreData &basic,
 319                                      const DerivedInfo &calculated,
 320                                      ThermalLocatorInfo &thermal_locator)
 321 {
 322   if (!thermal_locator.estimate_valid ||
 323       !basic.NavAltitudeAvailable() ||
 324       !calculated.last_thermal.IsDefined() ||
 325       negative(calculated.last_thermal.lift_rate))
 326     return;
 327
 328   if (calculated.wind_available &&
 329       calculated.wind.norm / calculated.last_thermal.lift_rate > fixed(10.0)) {
 330     // thermal strength is so weak compared to wind that source estimate
 331     // is unlikely to be reliable, so don't calculate or remember it
 332     return;
 333   }
 334
 335   GeoPoint ground_location;
 336   fixed ground_altitude = fixed(-1);
 337   EstimateThermalBase(terrain, thermal_locator.estimate_location,
 338                       basic.nav_altitude,
 339                       calculated.last_thermal.lift_rate,
 340                       calculated.GetWindOrZero(),
 341                       ground_location,
 342                       ground_altitude);
 343
 344   if (positive(ground_altitude)) {
 345     ThermalSource &source = thermal_locator.AllocateSource();
 346
 347     source.lift_rate = calculated.last_thermal.lift_rate;
 348     source.location = ground_location;
 349     source.ground_height = ground_altitude;
 350     source.time = basic.time;
 351   }
 352 }
 353
 354 inline void
 355 GlideComputerAirData::LastThermalStats(const MoreData &basic,
 356                                        DerivedInfo &calculated,
 357                                        bool last_circling)
 358 {
 359   if (calculated.circling || !last_circling ||
 360       !positive(calculated.climb_start_time))
 361     return;
 362
 363   fixed duration = calculated.cruise_start_time - calculated.climb_start_time;
 364   if (duration < THERMAL_TIME_MIN)
 365     return;
 366
 367   fixed gain =
 368     calculated.cruise_start_altitude_te - calculated.climb_start_altitude_te;
 369
 370   if (!positive(gain))
 371     return;
 372
 373   bool was_defined = calculated.last_thermal.IsDefined();
 374
 375   calculated.last_thermal.start_time = calculated.climb_start_time;
 376   calculated.last_thermal.end_time = calculated.cruise_start_time;
 377   calculated.last_thermal.gain = gain;
 378   calculated.last_thermal.duration = duration;
 379   calculated.last_thermal.CalculateLiftRate();
 380
 381   if (!was_defined)
 382     calculated.last_thermal_average_smooth =
 383         calculated.last_thermal.lift_rate;
 384   else
 385     calculated.last_thermal_average_smooth =
 386         LowPassFilter(calculated.last_thermal_average_smooth,
 387                       calculated.last_thermal.lift_rate, fixed(0.3));
 388
 389   ThermalSources(basic, calculated, calculated.thermal_locator);
 390 }
 391
 392 inline void
 393 GlideComputerAirData::ProcessSun(const NMEAInfo &basic,
 394                                  DerivedInfo &calculated,
 395                                  const ComputerSettings &settings)
 396 {
 397   if (!basic.location_available || !basic.date_available)
 398     return;
 399
 400   // Only calculate new azimuth if data is older than 15 minutes
 401   if (!calculated.sun_data_available.IsOlderThan(basic.clock, fixed(15 * 60)))
 402     return;
 403
 404   // Calculate new azimuth
 405   calculated.sun_azimuth =
 406     SunEphemeris::CalcAzimuth(basic.location, basic.date_time_utc,
 407                               settings.utc_offset);
 408   calculated.sun_data_available.Update(basic.clock);
 409 }
 410
 411 inline void
 412 GlideComputerAirData::NextLegEqThermal(const NMEAInfo &basic,
 413                                        DerivedInfo &calculated,
 414                                        const ComputerSettings &settings)
 415 {
 416   const GeoVector vector_remaining =
 417       calculated.task_stats.current_leg.vector_remaining;
 418   const GeoVector next_leg_vector =
 419       calculated.task_stats.current_leg.next_leg_vector;
 420
 421   if (!settings.polar.glide_polar_task.IsValid() ||
 422       !next_leg_vector.IsValid() ||
 423       !vector_remaining.IsValid() ||
 424       !calculated.wind_available) {
 425     // Assign a negative value to invalidate the result
 426     calculated.next_leg_eq_thermal = fixed(-1);
 427     return;
 428   }
 429
 430   // Calculate wind component on current and next legs
 431   const fixed wind_comp = calculated.wind.norm *
 432       (calculated.wind.bearing - vector_remaining.bearing).fastcosine();
 433   const fixed next_comp = calculated.wind.norm *
 434       (calculated.wind.bearing - next_leg_vector.bearing).fastcosine();
 435
 436   calculated.next_leg_eq_thermal =
 437       settings.polar.glide_polar_task.GetNextLegEqThermal(wind_comp, next_comp);
 438 }