src/Device/Driver/Leonardo.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 "Device/Driver/Leonardo.hpp"
  25 #include "Device/Parser.hpp"
  26 #include "Device/Driver.hpp"
  27 #include "NMEA/Info.hpp"
  28 #include "NMEA/InputLine.hpp"
  29 #include "Units/System.hpp"
  30 #include "Atmosphere/Temperature.hpp"
  31
  32 #include <stdlib.h>
  33 #include <math.h>
  34
  35 class LeonardoDevice : public AbstractDevice {
  36 public:
  37   virtual bool ParseNMEA(const char *line, struct NMEAInfo &info) override;
  38 };
  39
  40 static bool
  41 ReadSpeedVector(NMEAInputLine &line, SpeedVector &value_r)
  42 {
  43   fixed norm, bearing;
  44
  45   bool norm_valid = line.ReadChecked(norm);
  46   bool bearing_valid = line.ReadChecked(bearing);
  47
  48   if (bearing_valid && norm_valid) {
  49     value_r.norm = Units::ToSysUnit(norm, Unit::KILOMETER_PER_HOUR);
  50     value_r.bearing = Angle::Degrees(bearing);
  51     return true;
  52   } else
  53     return false;
  54 }
  55
  56 /**
  57  * Parse a "$C" sentence.
  58  *
  59  * Example: "$C,+2025,-7,+18,+25,+29,122,314,314,0,-356,+25,45,T*3D"
  60  */
  61 static bool
  62 LeonardoParseC(NMEAInputLine &line, NMEAInfo &info)
  63 {
  64   fixed value;
  65
  66   // 0 = altitude [m]
  67   if (line.ReadChecked(value))
  68     info.ProvideBaroAltitudeTrue(value);
  69
  70   // 1 = vario [cm/s]
  71   if (line.ReadChecked(value))
  72     info.ProvideTotalEnergyVario(value / 100);
  73
  74   // 2 = airspeed [km/h]
  75   /* XXX is that TAS or IAS? */
  76   if (line.ReadChecked(value))
  77     info.ProvideTrueAirspeed(Units::ToSysUnit(value, Unit::KILOMETER_PER_HOUR));
  78
  79   if (line.Rest().empty())
  80     /* short "$C" sentence ends after airspeed */
  81     return true;
  82
  83   // 3 = netto vario [dm/s]
  84   if (line.ReadChecked(value))
  85     info.ProvideNettoVario(value / 10);
  86
  87   // 4 = temperature [deg C]
  88   fixed oat;
  89   info.temperature_available = line.ReadChecked(oat);
  90   if (info.temperature_available)
  91     info.temperature = CelsiusToKelvin(oat);
  92
  93   line.Skip(5);
  94
  95   // 10 = wind speed [km/h]
  96   // 11 = wind direction [degrees]
  97   SpeedVector wind;
  98   if (ReadSpeedVector(line, wind))
  99     info.ProvideExternalWind(wind);
 100
 101   return true;
 102 }
 103
 104 /**
 105  * Parse a "$D" sentence.
 106  *
 107  * Example: "$D,+0,100554,+25,18,+31,,0,-356,+25,+11,115,96*6A"
 108  */
 109 static bool
 110 LeonardoParseD(NMEAInputLine &line, NMEAInfo &info)
 111 {
 112   fixed value;
 113
 114   // 0 = vario [dm/s]
 115   if (line.ReadChecked(value))
 116     info.ProvideTotalEnergyVario(value / 10);
 117
 118   if (line.Rest().empty())
 119     /* short "$D" sentence ends after vario */
 120     return true;
 121
 122   // 1 = air pressure [Pa]
 123   if (line.ReadChecked(value))
 124     info.ProvideStaticPressure(AtmosphericPressure::Pascal(value));
 125
 126   // 2 = netto vario [dm/s]
 127   if (line.ReadChecked(value))
 128     info.ProvideNettoVario(value / 10);
 129
 130   // 3 = airspeed [km/h]
 131   /* XXX is that TAS or IAS? */
 132   if (line.ReadChecked(value))
 133     info.ProvideTrueAirspeed(Units::ToSysUnit(value, Unit::KILOMETER_PER_HOUR));
 134
 135   // 4 = temperature [deg C]
 136   fixed oat;
 137   info.temperature_available = line.ReadChecked(oat);
 138   if (info.temperature_available)
 139     info.temperature = CelsiusToKelvin(oat);
 140
 141   // 5 = compass [degrees]
 142   /* XXX unsupported by XCSoar */
 143
 144   // 6 = optimal speed [km/h]
 145   /* XXX unsupported by XCSoar */
 146
 147   // 7 = equivalent MacCready [cm/s]
 148   /* XXX unsupported by XCSoar */
 149
 150   // 8 = wind speed [km/h]
 151   /* not used here, the "$C" record repeats it together with the
 152      direction */
 153
 154   return true;
 155 }
 156
 157 /**
 158  * Parse a "$PDGFTL1" sentence.
 159  *
 160  * Example: "$PDGFTL1,2025,2000,250,-14,45,134,28,65,382,153*3D"
 161  */
 162 static bool
 163 PDGFTL1(NMEAInputLine &line, NMEAInfo &info)
 164 {
 165   fixed value;
 166
 167   //  Baro Altitude QNE(1013.25)     2025     meter        2025 mt
 168   if (line.ReadChecked(value))
 169     info.ProvidePressureAltitude(value);
 170
 171   //  Baro Altitude QNH  2000     meter        2000 mt
 172   if (line.ReadChecked(value))
 173     info.ProvideBaroAltitudeTrue(value);
 174
 175   //  Vario  250      cm/sec       +2,50 m/s
 176   if (line.ReadChecked(value))
 177     info.ProvideTotalEnergyVario(value / 100);
 178
 179   //  Netto Vario  -14      dm/sec       -1,40 m/s
 180   if (line.ReadChecked(value))
 181     info.ProvideNettoVario(value / 10);
 182
 183   //  Indicated Air Speed  45       km/h         45 km/h
 184   if (line.ReadChecked(value))
 185     info.ProvideIndicatedAirspeed(Units::ToSysUnit(value, Unit::KILOMETER_PER_HOUR));
 186
 187   //  Ground Efficiency  134      ratio        13,4 : 1
 188   line.Skip();
 189
 190   //  Wind Speed  28       km/h         28 km/h
 191   //  Wind Direction  65       degree       65 degree
 192   SpeedVector wind;
 193   if (ReadSpeedVector(line, wind))
 194     info.ProvideExternalWind(wind);
 195
 196   //  Main Lithium Battery Voltage   382      0.01 volts   3,82 volts
 197   if (line.ReadChecked(value)) {
 198     info.voltage = value / 100;
 199     info.voltage_available.Update(info.clock);
 200   }
 201
 202   //  Backup AA Battery Voltage      153      0.01 volts   1,53 volts
 203
 204   return true;
 205 }
 206
 207 bool
 208 LeonardoDevice::ParseNMEA(const char *_line, NMEAInfo &info)
 209 {
 210   NMEAInputLine line(_line);
 211   char type[16];
 212   line.Read(type, 16);
 213
 214   if (StringIsEqual(type, "$C") ||
 215       StringIsEqual(type, "$c"))
 216     return LeonardoParseC(line, info);
 217
 218   else if (StringIsEqual(type, "$D") ||
 219            StringIsEqual(type, "$d"))
 220     return LeonardoParseD(line, info);
 221
 222   else if (StringIsEqual(type, "$PDGFTL1") ||
 223            StringIsEqual(type, "$PDGFTTL"))
 224     return PDGFTL1(line, info);
 225
 226   return false;
 227 }
 228
 229 static Device *
 230 LeonardoCreateOnPort(const DeviceConfig &config, Port &com_port)
 231 {
 232   return new LeonardoDevice();
 233 }
 234
 235 const struct DeviceRegister leonardo_driver = {
 236   _T("Leonardo"),
 237   _T("Digifly Leonardo"),
 238   0,
 239   LeonardoCreateOnPort,
 240 };