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-28
- Revision:
- 2:de84f8a0a706
- Parent:
- 1:4eadcb718c8b
File content as of revision 2:de84f8a0a706:
/* DJI Naza (v1, v1 Lite, V2) data decoder library from Pawelsky (c) 20141109 Refer to naza_decoder_wiring.jpg diagram for proper connection http://www.rcgroups.com/forums/showthread.php?t=1995704 Ported to mbed by Garfield38 20141128 Not for commercial use */ //#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; } }