src/Device/Driver/BlueFlyVario.cpp

   1 /*
   2 Copyright_License {
   3
   4   XCSoar Glide Computer - http://www.xcsoar.org/
   5   Copyright (C) 2000-2012 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 "Device/Driver/BlueFlyVario.hpp"
  25 #include "Device/Driver.hpp"
  26 #include "Math/KalmanFilter1d.hpp"
  27 #include "NMEA/Info.hpp"
  28
  29 #include <stdlib.h>
  30
  31 class BlueFlyDevice : public AbstractDevice {
  32 public:
  33   BlueFlyDevice();
  34
  35   void LinkTimeout() override;
  36
  37   virtual bool ParseNMEA(const char *line, struct NMEAInfo &info);
  38
  39   bool ParseBAT(const char *content, NMEAInfo &info);
  40   bool ParsePRS(const char *content, NMEAInfo &info);
  41 private:
  42   KalmanFilter1d kalman_filter;
  43 };
  44
  45 void
  46 BlueFlyDevice::LinkTimeout()
  47 {
  48   kalman_filter.Reset();
  49 }
  50
  51 bool
  52 BlueFlyDevice::ParseBAT(const char *content, NMEAInfo &info)
  53 {
  54   // e.g.
  55   // BAT 1234
  56
  57   char *endptr;
  58   int mV = (int)strtol(content, &endptr, 16);
  59   if (endptr == content) return true;
  60
  61   do {
  62     // piecewise linear approximation
  63     if (mV > 3900) {
  64       info.battery_level = fixed(70 + (mV - 3900)/10);
  65       break;
  66     }
  67     if (mV > 3700) {
  68       info.battery_level = fixed(4 + (mV - 3700)/3);
  69       break;
  70     }
  71     if (mV > 3600) {
  72       info.battery_level = fixed(0.04) * (mV - 3600);
  73       break;
  74     }
  75     // considered empty ...
  76     info.battery_level = fixed(0);
  77     break;
  78   }  while (0);
  79
  80   if (info.battery_level > fixed(100)) info.battery_level = fixed(100);
  81   info.battery_level_available.Update(info.clock);
  82
  83   return true;
  84 }
  85
  86 gcc_pure
  87 static inline
  88 fixed ComputeNoncompVario(const fixed pressure, const fixed d_pressure)
  89 {
  90   static constexpr fixed FACTOR(-2260.389548275485);
  91   static constexpr fixed EXPONENT(-0.8097374740609689);
  92   return fixed(FACTOR * pow(pressure, EXPONENT) * d_pressure);
  93 }
  94
  95 bool
  96 BlueFlyDevice::ParsePRS(const char *content, NMEAInfo &info)
  97 {
  98   // e.g. PRS 17CBA
  99
 100   char *endptr;
 101   long value = strtol(content, &endptr, 16);
 102   if (endptr != content) {
 103     AtmosphericPressure pressure = AtmosphericPressure::Pascal(fixed(value));
 104
 105     kalman_filter.Update(pressure.GetHectoPascal(), fixed(0.25), fixed(0.02));
 106
 107     info.ProvideNoncompVario(ComputeNoncompVario(kalman_filter.GetXAbs(),
 108                                                  kalman_filter.GetXVel()));
 109     info.ProvideStaticPressure(AtmosphericPressure::HectoPascal(kalman_filter.GetXAbs()));
 110   }
 111
 112   return true;
 113 }
 114
 115 bool
 116 BlueFlyDevice::ParseNMEA(const char *line, NMEAInfo &info)
 117 {
 118   if (memcmp(line, "PRS ", 4) == 0)
 119     return ParsePRS(line + 4, info);
 120   else if (memcmp(line, "BAT ", 4) == 0)
 121     return ParseBAT(line + 4, info);
 122   else
 123     return false;
 124 }
 125
 126 BlueFlyDevice::BlueFlyDevice()
 127 {
 128   kalman_filter.SetAccelerationVariance(fixed(0.3));
 129 }
 130
 131
 132 static Device *
 133 BlueFlyCreateOnPort(const DeviceConfig &config, Port &com_port)
 134 {
 135   return new BlueFlyDevice();
 136 }
 137
 138 const struct DeviceRegister bluefly_driver = {
 139   _T("BlueFly"),
 140   _T("BlueFly Vario"),
 141   0,
 142   BlueFlyCreateOnPort,
 143 };