src/Device/Driver/LevilAHRS_G.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/LevilAHRS_G.hpp"
  25 #include "Message.hpp"
  26 #include "Device/Driver.hpp"
  27 #include "NMEA/Info.hpp"
  28 #include "NMEA/InputLine.hpp"
  29 #include "Units/Units.hpp"
  30 #include <stdint.h>
  31
  32 static bool error_reported = false;
  33
  34 class LevilDevice : public AbstractDevice {
  35 public:
  36   virtual bool ParseNMEA(const char *line, struct NMEAInfo &info) override;
  37 };
  38
  39 static void
  40 ErrorMessage(gcc_unused unsigned code)
  41 {
  42   Message::AddMessage(_T("Levil AHRS: hardware error !"));
  43 }
  44
  45 static bool
  46 ParseRPYL(NMEAInputLine &line, NMEAInfo &info)
  47 {
  48   // $RPYL,Roll,Pitch,MagnHeading,SideSlip,YawRate,G,errorcode,
  49
  50   int roll;
  51   if (!line.ReadChecked(roll)) return false;
  52
  53   int pitch;
  54   if (!line.ReadChecked(pitch)) return false;
  55
  56   int heading;
  57   if (!line.ReadChecked(heading)) return false;
  58
  59   int sideslip;
  60   if (!line.ReadChecked(sideslip)) return false;
  61
  62   int yawrate;
  63   if (!line.ReadChecked(yawrate)) return false;
  64
  65   int G;
  66   if (!line.ReadChecked(G)) return false;
  67
  68   int errorcode;
  69   if (!line.ReadChecked(errorcode)) return false;
  70
  71   /* Error bits:
  72    *   0: Roll gyro test failed
  73    *   1: Roll gyro test failed
  74    *   2: Roll gyro test failed
  75    *   3: Acc X test failed
  76    *   4: Acc Y test failed
  77    *   5: Acc Z test failed
  78    *   6: Watchdog test failed
  79    *   7: Ram test failed
  80    *   8: EEPROM access test failed
  81    *   9: EEPROM checksum test failed
  82    *  10: Flash checksum test failed
  83    *  11: Low voltage error
  84    *  12: High temperature error (>60 C)
  85    *  13: Inconsistent roll data between gyro and acc.
  86    *  14: Inconsistent pitch data between gyro and acc.
  87    *  15: Inconsistent yaw data between gyro and acc.
  88    */
  89   if (errorcode && !error_reported) {
  90     ErrorMessage(errorcode);
  91     error_reported = true;
  92   }
  93
  94   info.attitude.bank_angle_available = true;
  95   info.attitude.bank_angle = Angle::Degrees(fixed(roll) / 10);
  96
  97   info.attitude.pitch_angle_available = true;
  98   info.attitude.pitch_angle = Angle::Degrees(fixed(pitch) / 10);
  99
 100   info.attitude.heading_available.Update(info.clock);
 101   info.attitude.heading = Angle::Degrees(fixed(heading) / 10);
 102
 103   info.acceleration.ProvideGLoad(fixed(G) / 1000, true);
 104
 105   return true;
 106 }
 107
 108 static bool
 109 ParseAPENV1(NMEAInputLine &line, NMEAInfo &info)
 110 {
 111   // $APENV1,IAS,Altitude,0,0,0,VerticalSpeed,
 112
 113   int ias;
 114   if (!line.ReadChecked(ias)) return false;
 115
 116   int altitude;
 117   if (!line.ReadChecked(altitude)) return false;
 118
 119   line.Skip();
 120   line.Skip();
 121   line.Skip();
 122
 123   // In ft/min, quality of this is limited, do not use for the time being
 124   int vs;
 125   if (!line.ReadChecked(vs)) return false;
 126
 127   fixed sys_alt = Units::ToSysUnit(fixed(altitude), Unit::FEET);
 128   info.ProvidePressureAltitude(sys_alt);
 129   info.ProvideIndicatedAirspeedWithAltitude(Units::ToSysUnit(fixed(ias), Unit::KNOTS), sys_alt);
 130
 131   return true;
 132 }
 133
 134 bool
 135 LevilDevice::ParseNMEA(const char *_line, NMEAInfo &info)
 136 {
 137   NMEAInputLine line(_line);
 138   char type[16];
 139   line.Read(type, 16);
 140
 141   if (error_reported) return false;
 142
 143   if (StringIsEqual(type, "$RPYL"))
 144     return ParseRPYL(line, info);
 145
 146   if (StringIsEqual(type, "$APENV1"))
 147     return ParseAPENV1(line, info);
 148
 149   return false;
 150 }
 151
 152 static Device *
 153 LevilCreateOnPort(const DeviceConfig &config, Port &com_port)
 154 {
 155   return new LevilDevice();
 156 }
 157
 158 const struct DeviceRegister levil_driver = {
 159   _T("Levil AHRS"),
 160   _T("Levil AHRS"),
 161   0,
 162   LevilCreateOnPort,
 163 };