src/Computer/Wind/MeasurementList.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 /* This library was originally imported from Cumulus
  25    http://kflog.org/cumulus/ */
  26
  27 #include "MeasurementList.hpp"
  28
  29 #include <stdlib.h>
  30 #include <algorithm>
  31
  32 /**
  33  * Returns the weighted mean windvector over the stored values, or 0
  34  * if no valid vector could be calculated (for instance: too little or
  35  * too low quality data).
  36  */
  37 const Vector
  38 WindMeasurementList::getWind(unsigned now, fixed alt, bool &found) const
  39 {
  40   //relative weight for each factor
  41   #define REL_FACTOR_QUALITY 100
  42   #define REL_FACTOR_ALTITUDE 100
  43   #define REL_FACTOR_TIME 200
  44   #define TIME_RANGE 36 // one hour
  45
  46   int altRange = 1000; //conf->getWindAltitudeRange();
  47   int timeRange = TIME_RANGE * 100; //conf->getWindTimeRange();
  48
  49   fixed k(0.0025);
  50
  51   unsigned int total_quality = 0;
  52
  53   Vector result(fixed(0), fixed(0));
  54
  55   found = false;
  56
  57   fixed override_time(1.1);
  58   bool overridden = false;
  59
  60   for (unsigned i = 0; i < measurements.size(); i++) {
  61     const WindMeasurement &m = measurements[i];
  62     fixed altdiff = (alt - m.altitude) / altRange;
  63     fixed timediff = fabs(fixed(now - m.time) / timeRange);
  64
  65     if ((fabs(altdiff) < fixed(1)) && (timediff < fixed(1))) {
  66       // measurement quality
  67       unsigned int q_quality = std::min(5, m.quality) * REL_FACTOR_QUALITY / 5;
  68
  69       // factor in altitude difference between current altitude and
  70       // measurement.  Maximum alt difference is 1000 m.
  71       unsigned int a_quality =
  72           iround(((fixed(2) / (altdiff * altdiff + fixed(1))) - fixed(1))
  73           * REL_FACTOR_ALTITUDE);
  74
  75       // factor in timedifference. Maximum difference is 1 hours.
  76       unsigned int t_quality =
  77           iround(k * (fixed(1) - timediff) / (timediff * timediff + k)
  78           * REL_FACTOR_TIME);
  79
  80       if (m.quality == 6) {
  81         if (timediff < override_time) {
  82           // over-ride happened, so re-set accumulator
  83           override_time = timediff;
  84           total_quality = 0;
  85           result.x = fixed(0);
  86           result.y = fixed(0);
  87           overridden = true;
  88         } else {
  89           // this isn't the latest over-ride or obtained fix, so ignore
  90           continue;
  91         }
  92       } else {
  93         if (timediff < override_time) {
  94           // a more recent fix was obtained than the over-ride, so start using
  95           // that one
  96           override_time = timediff;
  97           if (overridden) {
  98             // re-set accumulators
  99             overridden = false;
 100             total_quality = 0;
 101             result.x = fixed(0);
 102             result.y = fixed(0);
 103           }
 104         }
 105       }
 106
 107       unsigned int quality = q_quality * (a_quality * t_quality);
 108       result.x += m.vector.x * quality;
 109       result.y += m.vector.y * quality;
 110       total_quality += quality;
 111     }
 112   }
 113
 114   if (total_quality > 0) {
 115     found = true;
 116     result = Vector(result.x / (int)total_quality,
 117                     result.y / (int)total_quality);
 118   }
 119
 120   return result;
 121 }
 122
 123 /**
 124  * Adds the windvector vector with quality quality to the list.
 125  */
 126 void
 127 WindMeasurementList::addMeasurement(unsigned time, const SpeedVector &vector,
 128                                     fixed alt, int quality)
 129 {
 130   WindMeasurement &wind = measurements.full()
 131     ? measurements[getLeastImportantItem(time)] :
 132     measurements.append();
 133
 134   wind.vector = vector;
 135   wind.quality = quality;
 136   wind.altitude = alt;
 137   wind.time = time;
 138 }
 139
 140 /**
 141  * getLeastImportantItem is called to identify the item that should be
 142  * removed if the list is too full. Reimplemented from LimitedList.
 143  */
 144 unsigned
 145 WindMeasurementList::getLeastImportantItem(unsigned now)
 146 {
 147   unsigned maxscore = 0;
 148   unsigned int founditem = measurements.size() - 1;
 149
 150   for (int i = founditem; i >= 0; i--) {
 151     unsigned score = measurements[i].Score(now);
 152     if (score > maxscore) {
 153       maxscore = score;
 154       founditem = i;
 155     }
 156   }
 157
 158   return founditem;
 159 }
 160
 161 void
 162 WindMeasurementList::Reset()
 163 {
 164   measurements.clear();
 165 }