Diff: NazaDecoderLib.cpp

Revision:

1:4eadcb718c8b

Parent:

0:b0ba4e08a18c

Child:

2:de84f8a0a706

--- a/NazaDecoderLib.cpp	Tue Nov 18 16:39:57 2014 +0000
+++ b/NazaDecoderLib.cpp	Fri Nov 28 17:39:38 2014 +0000
@@ -4,17 +4,20 @@
   Not for commercial use
 
   Refer to naza_decoder_wiring.jpg diagram for proper connection
-
-  The RC PWM input code taken from https://www.instructables.com/id/RC-Quadrotor-Helicopter/step12/Arduino-Demo-PWM-Input/
+  http://www.rcgroups.com/forums/showthread.php?t=1995704
+  
 */
 
 //#include "Arduino.h"
+#include "mbed.h"
+
 #include "NazaDecoderLib.h"
 #include "stdint.h"
 #include "math.h"
 #define M_PI 3.14159265358979323846
 
 NazaDecoderLib NazaDecoder;
+Timer tn;
 
 NazaDecoderLib::NazaDecoderLib()
 {
@@ -24,16 +27,17 @@
     msgLen = 0;
     cs1 = 0;
     cs2 = 0;
+    syncError = 0;
 }
 
 int32_t NazaDecoderLib::decodeLong(uint8_t idx, uint8_t mask)
 {
     uint32_t variableToHoldValue = 0;
     unsigned char *pBytePointer;          //declare a byte-pointer
-    
+    // I suppose little-endian
     pBytePointer = (unsigned char *)&variableToHoldValue;
     for(int i = 0; i < 4; i++) {
-         *(pBytePointer+i) = gpsPayload[idx + i] ^ mask;
+        *(pBytePointer+i) = gpsPayload[idx + i] ^ mask;
     }
     return variableToHoldValue;
 }
@@ -42,7 +46,7 @@
 {
     uint16_t variableToHoldValue = 0;
     unsigned char *pBytePointer;          //declare a byte-pointer
-    
+
     pBytePointer = (unsigned char *)&variableToHoldValue;
     for(int i = 0; i < 2; i++) {
         *(pBytePointer+i) = gpsPayload[idx + i] ^ mask;
@@ -50,12 +54,20 @@
     return variableToHoldValue;
 }
 
-void NazaDecoderLib::updateCS(int input)
+void NazaDecoderLib::updateCS(uint8_t input)
 {
     cs1 += input;
     cs2 += cs1;
 }
 
+void NazaDecoderLib::getDebug(RawSerial &s, char* buf)
+{
+    sprintf(buf,"head:%3.0f lat:%f lon:%f alt:%4.1f sat:%d %d/%d/%d %d:%d:%d cycle:%d err:%d\n",\
+            headingNc, lat, lon, gpsAlt, sat, year, month, day, hour, minute,\
+            second, tn.read_ms(), syncError);
+    s.puts(buf);
+}
+
 double NazaDecoderLib::getLat()
 {
     return lat;
@@ -125,9 +137,10 @@
     return second;
 }
 
-uint8_t NazaDecoderLib::decode(int input)
+uint8_t NazaDecoderLib::decode(uint8_t input)
 {
     if((seq == 0) && (input == 0x55)) {
+        tn.reset();
         seq++;    // header (part 1 - 0x55)
     } else if((seq == 1) && (input == 0xAA)) {
         cs1 = 0;    // header (part 2 - 0xAA)
@@ -152,12 +165,14 @@
         seq++;    // verify checksum #1
     } else if((seq == 6) && (input == cs2)) {
         seq++;    // verify checksum #2
-    } else seq = 0;
-
+    } else {
+        tn.start();
+        seq = 0;
+        syncError++;
+    }
     if(seq == 7) { // all data in buffer
         seq = 0;
-        // Decode GPS data
-        if(msgId == NAZA_MESSAGE_GPS) {
+        if(msgId == NAZA_MESSAGE_GPS) {          // Decode GPS data
             uint8_t mask = gpsPayload[55];
             uint32_t time = decodeLong(0, mask);
             second = time & 0x3f;
@@ -199,6 +214,7 @@
                     fix = NO_FIX;
                     break;
             }
+            // change baud rate before sending akeru command
             if((fix != NO_FIX) && (fixFlags & 0x02)) fix = FIX_DGPS;
         }
         // Decode compass data (not tilt compensated)
@@ -214,6 +230,7 @@
             headingNc = -atan2(y - ((magYMax + magYMin) / 2.0), x - ((magXMax + magXMin) / 2.0) ) * 180.0 / M_PI;
             if(headingNc < 0) headingNc += 360.0;
         }
+        syncError = 0;
         return msgId;
     } else {
         return NAZA_MESSAGE_NONE;