http://www.rcgroups.com/forums/showthread.php?t=1995704
Use the following image to get GPS signal and supply:
NazaDecoderLib.cpp
- Committer:
- garfield38
- Date:
- 2014-11-18
- Revision:
- 0:b0ba4e08a18c
- Child:
- 1:4eadcb718c8b
File content as of revision 0:b0ba4e08a18c:
/* 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; } }