File content as of revision 1:4eadcb718c8b:

/*
  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
  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()
{
    seq = 0;
    cnt = 0;
    msgId = 0;
    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;
    }
    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(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;
}
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(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)
        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 {
        tn.start();
        seq = 0;
        syncError++;
    }
    if(seq == 7) { // all data in buffer
        seq = 0;
        if(msgId == NAZA_MESSAGE_GPS) {          // Decode GPS data
            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;
            }
            // change baud rate before sending akeru command
            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;
        }
        syncError = 0;
        return msgId;
    } else {
        return NAZA_MESSAGE_NONE;
    }
}