Diff: NazaDecoderLib.cpp

Revision:

0:b0ba4e08a18c

Child:

1:4eadcb718c8b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NazaDecoderLib.cpp	Tue Nov 18 16:39:57 2014 +0000
@@ -0,0 +1,221 @@
+/*
+  DJI Naza (v1, v1 Lite, V2) data decoder library
+  (c) Pawelsky 20141109
+  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/
+*/
+
+//#include "Arduino.h"
+#include "NazaDecoderLib.h"
+#include "stdint.h"
+#include "math.h"
+#define M_PI 3.14159265358979323846
+
+NazaDecoderLib NazaDecoder;
+
+NazaDecoderLib::NazaDecoderLib()
+{
+    seq = 0;
+    cnt = 0;
+    msgId = 0;
+    msgLen = 0;
+    cs1 = 0;
+    cs2 = 0;
+}
+
+int32_t NazaDecoderLib::decodeLong(uint8_t idx, uint8_t mask)
+{
+    uint32_t variableToHoldValue = 0;
+    unsigned char *pBytePointer;          //declare a byte-pointer
+    
+    pBytePointer = (unsigned char *)&variableToHoldValue;
+    for(int i = 0; i < 4; i++) {
+         *(pBytePointer+i) = gpsPayload[idx + i] ^ mask;
+    }
+    return variableToHoldValue;
+}
+
+int16_t NazaDecoderLib::decodeShort(uint8_t idx, uint8_t mask)
+{
+    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;
+    }
+    return variableToHoldValue;
+}
+
+void NazaDecoderLib::updateCS(int input)
+{
+    cs1 += input;
+    cs2 += cs1;
+}
+
+double NazaDecoderLib::getLat()
+{
+    return lat;
+}
+double NazaDecoderLib::getLon()
+{
+    return lon;
+}
+double NazaDecoderLib::getGpsAlt()
+{
+    return gpsAlt;
+}
+double NazaDecoderLib::getSpeed()
+{
+    return spd;
+}
+NazaDecoderLib::fixType_t  NazaDecoderLib::getFixType()
+{
+    return fix;
+}
+uint8_t NazaDecoderLib::getNumSat()
+{
+    return sat;
+}
+double NazaDecoderLib::getHeadingNc()
+{
+    return headingNc;
+}
+double NazaDecoderLib::getCog()
+{
+    return cog;
+}
+double NazaDecoderLib::getGpsVsi()
+{
+    return gpsVsi;
+}
+double NazaDecoderLib::getHdop()
+{
+    return hdop;
+}
+double NazaDecoderLib::getVdop()
+{
+    return vdop;
+}
+uint8_t NazaDecoderLib::getYear()
+{
+    return year;
+}
+uint8_t NazaDecoderLib::getMonth()
+{
+    return month;
+}
+uint8_t NazaDecoderLib::getDay()
+{
+    return day;
+}
+uint8_t NazaDecoderLib::getHour()
+{
+    return hour;
+}
+uint8_t NazaDecoderLib::getMinute()
+{
+    return minute;
+}
+uint8_t NazaDecoderLib::getSecond()
+{
+    return second;
+}
+
+uint8_t NazaDecoderLib::decode(int input)
+{
+    if((seq == 0) && (input == 0x55)) {
+        seq++;    // header (part 1 - 0x55)
+    } else if((seq == 1) && (input == 0xAA)) {
+        cs1 = 0;    // header (part 2 - 0xAA)
+        cs2 = 0;
+        seq++;
+    } else if((seq == 2) && ((input == 0x10) || (input == 0x20))) {
+        msgId = input;    // message id
+        updateCS(input);
+        seq++;
+    } else if(seq == 3) {
+        msgLen = input;    // message payload lenght
+        cnt = 0;
+        updateCS(input);
+        seq++;
+    } else if(seq == 4) {
+        gpsPayload[cnt++] = input;    // store payload in buffer
+        updateCS(input);
+        if(cnt >= msgLen) {
+            seq++;
+        }
+    } else if((seq == 5) && (input == cs1)) {
+        seq++;    // verify checksum #1
+    } else if((seq == 6) && (input == cs2)) {
+        seq++;    // verify checksum #2
+    } else seq = 0;
+
+    if(seq == 7) { // all data in buffer
+        seq = 0;
+        // Decode GPS data
+        if(msgId == NAZA_MESSAGE_GPS) {
+            uint8_t mask = gpsPayload[55];
+            uint32_t time = decodeLong(0, mask);
+            second = time & 0x3f;
+            time >>= 6;
+            minute = time & 0x3f;
+            time >>= 6;
+            hour = time & 0x0f;
+            time >>= 4;
+            day = time & 0x1f;
+            time >>= 5;
+            if(hour > 7) day++;
+            month = time & 0x0f;
+            time >>= 4;
+            year = time & 0x3f;
+            lon = (double)decodeLong(4, mask) / 10000000;
+            lat = (double)decodeLong(8, mask) / 10000000;
+            gpsAlt = (double)decodeLong(12, mask) / 1000;
+            double nVel = (double)decodeLong(28, mask) / 100;
+            double eVel = (double)decodeLong(32, mask) / 100;
+            spd = sqrt(nVel * nVel + eVel * eVel);
+            cog = atan2(eVel, nVel) * 180.0 / M_PI;
+            if(cog < 0) cog += 360.0;
+            gpsVsi = -(double)decodeLong(36, mask) / 100;
+            vdop = (double)decodeShort(42, mask) / 100;
+            double ndop = (double)decodeShort(44, mask) / 100;
+            double edop = (double)decodeShort(46, mask) / 100;
+            hdop = sqrt(ndop * ndop + edop * edop);
+            sat  = gpsPayload[48];
+            uint8_t fixType = gpsPayload[50] ^ mask;
+            uint8_t fixFlags = gpsPayload[52] ^ mask;
+            switch(fixType) {
+                case 2 :
+                    fix = FIX_2D;
+                    break;
+                case 3 :
+                    fix = FIX_3D;
+                    break;
+                default:
+                    fix = NO_FIX;
+                    break;
+            }
+            if((fix != NO_FIX) && (fixFlags & 0x02)) fix = FIX_DGPS;
+        }
+        // Decode compass data (not tilt compensated)
+        else if (msgId == NAZA_MESSAGE_COMPASS) {
+            uint8_t mask = gpsPayload[4];
+            mask = (((mask ^ (mask >> 4)) & 0x0F) | ((mask << 3) & 0xF0)) ^ (((mask & 0x01) << 3) | ((mask & 0x01) << 7));
+            int16_t x = decodeShort(0, mask);
+            int16_t y = decodeShort(2, mask);
+            if(x > magXMax) magXMax = x;
+            if(x < magXMin) magXMin = x;
+            if(y > magYMax) magYMax = y;
+            if(y < magYMin) magYMin = y;
+            headingNc = -atan2(y - ((magYMax + magYMin) / 2.0), x - ((magXMax + magXMin) / 2.0) ) * 180.0 / M_PI;
+            if(headingNc < 0) headingNc += 360.0;
+        }
+        return msgId;
+    } else {
+        return NAZA_MESSAGE_NONE;
+    }
+}