http://www.rcgroups.com/forums/showthread.php?t=1995704
Use the following image to get GPS signal and supply:
Diff: NazaDecoderLib.cpp
- Revision:
- 0:b0ba4e08a18c
- Child:
- 1:4eadcb718c8b
diff -r 000000000000 -r b0ba4e08a18c NazaDecoderLib.cpp --- /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; + } +}