refactoring & formatting

zendes · Feb 7, 2014 · e62e82b · e62e82b
1 parent d5ceb80
commit e62e82b
Show file tree

Hide file tree

Showing 2 changed files with 54 additions and 70 deletions.
diff --git a/SPort.cpp b/SPort.cpp
@@ -1,13 +1,11 @@
 #include "SPort.h"
 #include <Arduino.h>
 
-void SPort::begin()
-{
+void SPort::begin() {
   SPort::begin(true);
 }
 
-void SPort::begin(bool useTimer)
-{
+void SPort::begin(bool useTimer) {
   if (useTimer) {
     noInterrupts();
     TCCR2A  = 0;
@@ -25,12 +23,13 @@ void SPort::begin(bool useTimer)
   _vfasVoltage     = 0;
   _vfasCurrent     = 0;
   _vfasConsumption = 0;
+  _uptime          = 0;
+  _altitudeOffset  = 0;
 
   _serial.begin(57600, SERIAL_8N1);
 }
 
-void SPort::process()
-{
+void SPort::process() {
   static byte buffer[FRSKY_RX_PACKET_SIZE];
   static byte bufferIndex = 0;
   static byte dataState = STATE_DATA_IDLE;
@@ -57,63 +56,50 @@ void SPort::process()
     }
 
     if (bufferIndex == FRSKY_RX_PACKET_SIZE) {
-      processSportPacket(buffer);
       dataState = STATE_DATA_IDLE;
-    }
-  }
-}
-
-bool SPort::checkSportPacket(byte *packet) {
-  short crc = 0;
-  for (int i = 1; i < FRSKY_RX_PACKET_SIZE; i++) {
-    crc += packet[i];
-    crc += crc >> 8;
-    crc &= 0x00ff;
-    crc += crc >> 8;
-    crc &= 0x00ff;
-  }
-  return (crc == 0x00ff);
-}
-
-void SPort::processSportPacket(byte *packet) {
-  if (!checkSportPacket(packet))
-    return;
 
-  byte packetType = packet[1];
-  unsigned int appId = *((unsigned int *)(packet+2));
+      short crc = 0;
+      for (int i = 1; i < FRSKY_RX_PACKET_SIZE; i++) {
+        crc += buffer[i];
+        crc += crc >> 8;
+        crc &= 0x00ff;
+        crc += crc >> 8;
+        crc &= 0x00ff;
+      }
+      if (crc == 0x00ff) {
+        byte packetType = buffer[1];
+        switch (packetType) {
+          case DATA_FRAME:
+            unsigned int appId = *((unsigned int *)(buffer+2));
 
-  switch (packetType) {
-    case DATA_FRAME:
+            if (appId >= VARIO_FIRST_ID && appId <= VARIO_LAST_ID) {
+              _varioSpeed = SPORT_DATA_S32(buffer);
 
-      if (appId >= VARIO_FIRST_ID && appId <= VARIO_LAST_ID) {
-        _varioSpeed = SPORT_DATA_S32(packet);
+            } else if (appId >= ALT_FIRST_ID && appId <= ALT_LAST_ID) {
+                if (_altitudeOffset == 0)
+                  _altitudeOffset = -SPORT_DATA_S32(buffer);
 
-      } else if (appId >= ALT_FIRST_ID && appId <= ALT_LAST_ID) {
-        _varioAltitude = SPort::altitude(SPORT_DATA_S32(packet));
+              _varioAltitude = SPORT_DATA_S32(buffer) + _altitudeOffset;
 
-      } else if (appId >= VFAS_FIRST_ID && appId <= VFAS_LAST_ID) {
-        _vfasVoltage = SPORT_DATA_S32(packet);
+            } else if (appId >= VFAS_FIRST_ID && appId <= VFAS_LAST_ID) {
+              _vfasVoltage = SPORT_DATA_S32(buffer) * 10;
 
-      } else if (appId >= CURR_FIRST_ID && appId <= CURR_LAST_ID) {
-        _vfasCurrent = SPORT_DATA_U32(packet);
+            } else if (appId >= CURR_FIRST_ID && appId <= CURR_LAST_ID) {
+              _vfasCurrent = SPORT_DATA_U32(buffer) * 100;
 
-      } else if (appId >= FUEL_FIRST_ID && appId <= FUEL_LAST_ID) {
-        _vfasConsumption = SPORT_DATA_U32(packet);
+              unsigned long now = micros();
+              _vfasConsumption += (long)_vfasCurrent * 1000 / (3600000000 /
+                                                              (now - _uptime));
+              _uptime = now;
 
+            }
+            break;
+        }
       }
-      break;
+    }
   }
 }
 
-long SPort::altitude(long altiData) {
-  static long altiOffset = 0;
-
-  if (altiOffset == 0)
-    altiOffset = -altiData;
-
-  return altiData + altiOffset;
-}
-
 long SPort::getVarioSpeed() {
   return _varioSpeed;
 }
@@ -122,14 +108,14 @@ long SPort::getVarioAltitude() {
   return _varioAltitude;
 }
 
-long SPort::getVfasVoltage(){
+int SPort::getVfasVoltage(){
   return _vfasVoltage;
 }
 
-long SPort::getVfasCurrent() {
+int SPort::getVfasCurrent() {
   return _vfasCurrent;
 }
 
-long SPort::getVfasConsumption() {
-  return _vfasConsumption;
+int SPort::getVfasConsumption() {
+  return _vfasConsumption / 1000;
 }
diff --git a/SPort.h b/SPort.h
@@ -47,32 +47,30 @@
 #define CELLS_FIRST_ID     0x0300
 #define CELLS_LAST_ID      0x030f
 
-#define SPORT_DATA_U8(packet)   (packet[4])
-#define SPORT_DATA_S32(packet)  (*((int32_t *)(packet+4)))
-#define SPORT_DATA_U32(packet)  (*((uint32_t *)(packet+4)))
+#define SPORT_DATA_U8(packet)   (buffer[4])
+#define SPORT_DATA_S32(packet)  (*((int32_t *)(buffer+4)))
+#define SPORT_DATA_U32(packet)  (*((uint32_t *)(buffer+4)))
 
-class SPort
-{
+class SPort {
   public:
     SPort(HardwareSerial & serial) : _serial (serial) {}
     void begin();
     void begin(bool useTimer);
     void process();
     long getVarioSpeed();
     long getVarioAltitude();
-    long getVfasVoltage();
-    long getVfasCurrent();
-    long getVfasConsumption();
+    int  getVfasVoltage();
+    int  getVfasCurrent();
+    int  getVfasConsumption();
   private:
     HardwareSerial & _serial;
-    bool checkSportPacket(byte *packet);
-    void processSportPacket(byte *packet);
-    long altitude(long altiData);
-    long _varioSpeed;
-    long _varioAltitude;
-    long _vfasVoltage;
-    long _vfasCurrent;
-    long _vfasConsumption;
+    long          _varioSpeed;
+    long          _varioAltitude;
+    unsigned long _altitudeOffset;
+    int           _vfasVoltage;
+    int           _vfasCurrent;
+    unsigned long _vfasConsumption;
+    unsigned long _uptime;
 };
 
 #endif