Stéphane Driussi / NazaDecoder

http://www.rcgroups.com/forums/showthread.php?t=1995704

Use the following image to get GPS signal and supply:

http://static.rcgroups.net/forums/attachments/3/9/3/9/3/6/a6132334-125-naza_gps_pinout.jpg

NazaDecoderLib.cpp@1:4eadcb718c8b, 2014-11-28 (annotated)

Committer:
garfield38
Date:
Fri Nov 28 17:39:38 2014 +0000
Revision:
1:4eadcb718c8b
Parent:
0:b0ba4e08a18c
Child:
2:de84f8a0a706
Ready for use

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garfield38 0:b0ba4e08a18c 1 /*
garfield38 0:b0ba4e08a18c 2 DJI Naza (v1, v1 Lite, V2) data decoder library
garfield38 0:b0ba4e08a18c 3 (c) Pawelsky 20141109
garfield38 0:b0ba4e08a18c 4 Not for commercial use
garfield38 0:b0ba4e08a18c 5
garfield38 0:b0ba4e08a18c 6 Refer to naza_decoder_wiring.jpg diagram for proper connection
garfield38 1:4eadcb718c8b 7 http://www.rcgroups.com/forums/showthread.php?t=1995704
garfield38 1:4eadcb718c8b 8
garfield38 0:b0ba4e08a18c 9 */
garfield38 0:b0ba4e08a18c 10
garfield38 0:b0ba4e08a18c 11 //#include "Arduino.h"
garfield38 1:4eadcb718c8b 12 #include "mbed.h"
garfield38 1:4eadcb718c8b 13
garfield38 0:b0ba4e08a18c 14 #include "NazaDecoderLib.h"
garfield38 0:b0ba4e08a18c 15 #include "stdint.h"
garfield38 0:b0ba4e08a18c 16 #include "math.h"
garfield38 0:b0ba4e08a18c 17 #define M_PI 3.14159265358979323846
garfield38 0:b0ba4e08a18c 18
garfield38 0:b0ba4e08a18c 19 NazaDecoderLib NazaDecoder;
garfield38 1:4eadcb718c8b 20 Timer tn;
garfield38 0:b0ba4e08a18c 21
garfield38 0:b0ba4e08a18c 22 NazaDecoderLib::NazaDecoderLib()
garfield38 0:b0ba4e08a18c 23 {
garfield38 0:b0ba4e08a18c 24 seq = 0;
garfield38 0:b0ba4e08a18c 25 cnt = 0;
garfield38 0:b0ba4e08a18c 26 msgId = 0;
garfield38 0:b0ba4e08a18c 27 msgLen = 0;
garfield38 0:b0ba4e08a18c 28 cs1 = 0;
garfield38 0:b0ba4e08a18c 29 cs2 = 0;
garfield38 1:4eadcb718c8b 30 syncError = 0;
garfield38 0:b0ba4e08a18c 31 }
garfield38 0:b0ba4e08a18c 32
garfield38 0:b0ba4e08a18c 33 int32_t NazaDecoderLib::decodeLong(uint8_t idx, uint8_t mask)
garfield38 0:b0ba4e08a18c 34 {
garfield38 0:b0ba4e08a18c 35 uint32_t variableToHoldValue = 0;
garfield38 0:b0ba4e08a18c 36 unsigned char *pBytePointer; //declare a byte-pointer
garfield38 1:4eadcb718c8b 37 // I suppose little-endian
garfield38 0:b0ba4e08a18c 38 pBytePointer = (unsigned char *)&variableToHoldValue;
garfield38 0:b0ba4e08a18c 39 for(int i = 0; i < 4; i++) {
garfield38 1:4eadcb718c8b 40 *(pBytePointer+i) = gpsPayload[idx + i] ^ mask;
garfield38 0:b0ba4e08a18c 41 }
garfield38 0:b0ba4e08a18c 42 return variableToHoldValue;
garfield38 0:b0ba4e08a18c 43 }
garfield38 0:b0ba4e08a18c 44
garfield38 0:b0ba4e08a18c 45 int16_t NazaDecoderLib::decodeShort(uint8_t idx, uint8_t mask)
garfield38 0:b0ba4e08a18c 46 {
garfield38 0:b0ba4e08a18c 47 uint16_t variableToHoldValue = 0;
garfield38 0:b0ba4e08a18c 48 unsigned char *pBytePointer; //declare a byte-pointer
garfield38 1:4eadcb718c8b 49
garfield38 0:b0ba4e08a18c 50 pBytePointer = (unsigned char *)&variableToHoldValue;
garfield38 0:b0ba4e08a18c 51 for(int i = 0; i < 2; i++) {
garfield38 0:b0ba4e08a18c 52 *(pBytePointer+i) = gpsPayload[idx + i] ^ mask;
garfield38 0:b0ba4e08a18c 53 }
garfield38 0:b0ba4e08a18c 54 return variableToHoldValue;
garfield38 0:b0ba4e08a18c 55 }
garfield38 0:b0ba4e08a18c 56
garfield38 1:4eadcb718c8b 57 void NazaDecoderLib::updateCS(uint8_t input)
garfield38 0:b0ba4e08a18c 58 {
garfield38 0:b0ba4e08a18c 59 cs1 += input;
garfield38 0:b0ba4e08a18c 60 cs2 += cs1;
garfield38 0:b0ba4e08a18c 61 }
garfield38 0:b0ba4e08a18c 62
garfield38 1:4eadcb718c8b 63 void NazaDecoderLib::getDebug(RawSerial &s, char* buf)
garfield38 1:4eadcb718c8b 64 {
garfield38 1:4eadcb718c8b 65 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",\
garfield38 1:4eadcb718c8b 66 headingNc, lat, lon, gpsAlt, sat, year, month, day, hour, minute,\
garfield38 1:4eadcb718c8b 67 second, tn.read_ms(), syncError);
garfield38 1:4eadcb718c8b 68 s.puts(buf);
garfield38 1:4eadcb718c8b 69 }
garfield38 1:4eadcb718c8b 70
garfield38 0:b0ba4e08a18c 71 double NazaDecoderLib::getLat()
garfield38 0:b0ba4e08a18c 72 {
garfield38 0:b0ba4e08a18c 73 return lat;
garfield38 0:b0ba4e08a18c 74 }
garfield38 0:b0ba4e08a18c 75 double NazaDecoderLib::getLon()
garfield38 0:b0ba4e08a18c 76 {
garfield38 0:b0ba4e08a18c 77 return lon;
garfield38 0:b0ba4e08a18c 78 }
garfield38 0:b0ba4e08a18c 79 double NazaDecoderLib::getGpsAlt()
garfield38 0:b0ba4e08a18c 80 {
garfield38 0:b0ba4e08a18c 81 return gpsAlt;
garfield38 0:b0ba4e08a18c 82 }
garfield38 0:b0ba4e08a18c 83 double NazaDecoderLib::getSpeed()
garfield38 0:b0ba4e08a18c 84 {
garfield38 0:b0ba4e08a18c 85 return spd;
garfield38 0:b0ba4e08a18c 86 }
garfield38 0:b0ba4e08a18c 87 NazaDecoderLib::fixType_t NazaDecoderLib::getFixType()
garfield38 0:b0ba4e08a18c 88 {
garfield38 0:b0ba4e08a18c 89 return fix;
garfield38 0:b0ba4e08a18c 90 }
garfield38 0:b0ba4e08a18c 91 uint8_t NazaDecoderLib::getNumSat()
garfield38 0:b0ba4e08a18c 92 {
garfield38 0:b0ba4e08a18c 93 return sat;
garfield38 0:b0ba4e08a18c 94 }
garfield38 0:b0ba4e08a18c 95 double NazaDecoderLib::getHeadingNc()
garfield38 0:b0ba4e08a18c 96 {
garfield38 0:b0ba4e08a18c 97 return headingNc;
garfield38 0:b0ba4e08a18c 98 }
garfield38 0:b0ba4e08a18c 99 double NazaDecoderLib::getCog()
garfield38 0:b0ba4e08a18c 100 {
garfield38 0:b0ba4e08a18c 101 return cog;
garfield38 0:b0ba4e08a18c 102 }
garfield38 0:b0ba4e08a18c 103 double NazaDecoderLib::getGpsVsi()
garfield38 0:b0ba4e08a18c 104 {
garfield38 0:b0ba4e08a18c 105 return gpsVsi;
garfield38 0:b0ba4e08a18c 106 }
garfield38 0:b0ba4e08a18c 107 double NazaDecoderLib::getHdop()
garfield38 0:b0ba4e08a18c 108 {
garfield38 0:b0ba4e08a18c 109 return hdop;
garfield38 0:b0ba4e08a18c 110 }
garfield38 0:b0ba4e08a18c 111 double NazaDecoderLib::getVdop()
garfield38 0:b0ba4e08a18c 112 {
garfield38 0:b0ba4e08a18c 113 return vdop;
garfield38 0:b0ba4e08a18c 114 }
garfield38 0:b0ba4e08a18c 115 uint8_t NazaDecoderLib::getYear()
garfield38 0:b0ba4e08a18c 116 {
garfield38 0:b0ba4e08a18c 117 return year;
garfield38 0:b0ba4e08a18c 118 }
garfield38 0:b0ba4e08a18c 119 uint8_t NazaDecoderLib::getMonth()
garfield38 0:b0ba4e08a18c 120 {
garfield38 0:b0ba4e08a18c 121 return month;
garfield38 0:b0ba4e08a18c 122 }
garfield38 0:b0ba4e08a18c 123 uint8_t NazaDecoderLib::getDay()
garfield38 0:b0ba4e08a18c 124 {
garfield38 0:b0ba4e08a18c 125 return day;
garfield38 0:b0ba4e08a18c 126 }
garfield38 0:b0ba4e08a18c 127 uint8_t NazaDecoderLib::getHour()
garfield38 0:b0ba4e08a18c 128 {
garfield38 0:b0ba4e08a18c 129 return hour;
garfield38 0:b0ba4e08a18c 130 }
garfield38 0:b0ba4e08a18c 131 uint8_t NazaDecoderLib::getMinute()
garfield38 0:b0ba4e08a18c 132 {
garfield38 0:b0ba4e08a18c 133 return minute;
garfield38 0:b0ba4e08a18c 134 }
garfield38 0:b0ba4e08a18c 135 uint8_t NazaDecoderLib::getSecond()
garfield38 0:b0ba4e08a18c 136 {
garfield38 0:b0ba4e08a18c 137 return second;
garfield38 0:b0ba4e08a18c 138 }
garfield38 0:b0ba4e08a18c 139
garfield38 1:4eadcb718c8b 140 uint8_t NazaDecoderLib::decode(uint8_t input)
garfield38 0:b0ba4e08a18c 141 {
garfield38 0:b0ba4e08a18c 142 if((seq == 0) && (input == 0x55)) {
garfield38 1:4eadcb718c8b 143 tn.reset();
garfield38 0:b0ba4e08a18c 144 seq++; // header (part 1 - 0x55)
garfield38 0:b0ba4e08a18c 145 } else if((seq == 1) && (input == 0xAA)) {
garfield38 0:b0ba4e08a18c 146 cs1 = 0; // header (part 2 - 0xAA)
garfield38 0:b0ba4e08a18c 147 cs2 = 0;
garfield38 0:b0ba4e08a18c 148 seq++;
garfield38 0:b0ba4e08a18c 149 } else if((seq == 2) && ((input == 0x10) || (input == 0x20))) {
garfield38 0:b0ba4e08a18c 150 msgId = input; // message id
garfield38 0:b0ba4e08a18c 151 updateCS(input);
garfield38 0:b0ba4e08a18c 152 seq++;
garfield38 0:b0ba4e08a18c 153 } else if(seq == 3) {
garfield38 0:b0ba4e08a18c 154 msgLen = input; // message payload lenght
garfield38 0:b0ba4e08a18c 155 cnt = 0;
garfield38 0:b0ba4e08a18c 156 updateCS(input);
garfield38 0:b0ba4e08a18c 157 seq++;
garfield38 0:b0ba4e08a18c 158 } else if(seq == 4) {
garfield38 0:b0ba4e08a18c 159 gpsPayload[cnt++] = input; // store payload in buffer
garfield38 0:b0ba4e08a18c 160 updateCS(input);
garfield38 0:b0ba4e08a18c 161 if(cnt >= msgLen) {
garfield38 0:b0ba4e08a18c 162 seq++;
garfield38 0:b0ba4e08a18c 163 }
garfield38 0:b0ba4e08a18c 164 } else if((seq == 5) && (input == cs1)) {
garfield38 0:b0ba4e08a18c 165 seq++; // verify checksum #1
garfield38 0:b0ba4e08a18c 166 } else if((seq == 6) && (input == cs2)) {
garfield38 0:b0ba4e08a18c 167 seq++; // verify checksum #2
garfield38 1:4eadcb718c8b 168 } else {
garfield38 1:4eadcb718c8b 169 tn.start();
garfield38 1:4eadcb718c8b 170 seq = 0;
garfield38 1:4eadcb718c8b 171 syncError++;
garfield38 1:4eadcb718c8b 172 }
garfield38 0:b0ba4e08a18c 173 if(seq == 7) { // all data in buffer
garfield38 0:b0ba4e08a18c 174 seq = 0;
garfield38 1:4eadcb718c8b 175 if(msgId == NAZA_MESSAGE_GPS) { // Decode GPS data
garfield38 0:b0ba4e08a18c 176 uint8_t mask = gpsPayload[55];
garfield38 0:b0ba4e08a18c 177 uint32_t time = decodeLong(0, mask);
garfield38 0:b0ba4e08a18c 178 second = time & 0x3f;
garfield38 0:b0ba4e08a18c 179 time >>= 6;
garfield38 0:b0ba4e08a18c 180 minute = time & 0x3f;
garfield38 0:b0ba4e08a18c 181 time >>= 6;
garfield38 0:b0ba4e08a18c 182 hour = time & 0x0f;
garfield38 0:b0ba4e08a18c 183 time >>= 4;
garfield38 0:b0ba4e08a18c 184 day = time & 0x1f;
garfield38 0:b0ba4e08a18c 185 time >>= 5;
garfield38 0:b0ba4e08a18c 186 if(hour > 7) day++;
garfield38 0:b0ba4e08a18c 187 month = time & 0x0f;
garfield38 0:b0ba4e08a18c 188 time >>= 4;
garfield38 0:b0ba4e08a18c 189 year = time & 0x3f;
garfield38 0:b0ba4e08a18c 190 lon = (double)decodeLong(4, mask) / 10000000;
garfield38 0:b0ba4e08a18c 191 lat = (double)decodeLong(8, mask) / 10000000;
garfield38 0:b0ba4e08a18c 192 gpsAlt = (double)decodeLong(12, mask) / 1000;
garfield38 0:b0ba4e08a18c 193 double nVel = (double)decodeLong(28, mask) / 100;
garfield38 0:b0ba4e08a18c 194 double eVel = (double)decodeLong(32, mask) / 100;
garfield38 0:b0ba4e08a18c 195 spd = sqrt(nVel * nVel + eVel * eVel);
garfield38 0:b0ba4e08a18c 196 cog = atan2(eVel, nVel) * 180.0 / M_PI;
garfield38 0:b0ba4e08a18c 197 if(cog < 0) cog += 360.0;
garfield38 0:b0ba4e08a18c 198 gpsVsi = -(double)decodeLong(36, mask) / 100;
garfield38 0:b0ba4e08a18c 199 vdop = (double)decodeShort(42, mask) / 100;
garfield38 0:b0ba4e08a18c 200 double ndop = (double)decodeShort(44, mask) / 100;
garfield38 0:b0ba4e08a18c 201 double edop = (double)decodeShort(46, mask) / 100;
garfield38 0:b0ba4e08a18c 202 hdop = sqrt(ndop * ndop + edop * edop);
garfield38 0:b0ba4e08a18c 203 sat = gpsPayload[48];
garfield38 0:b0ba4e08a18c 204 uint8_t fixType = gpsPayload[50] ^ mask;
garfield38 0:b0ba4e08a18c 205 uint8_t fixFlags = gpsPayload[52] ^ mask;
garfield38 0:b0ba4e08a18c 206 switch(fixType) {
garfield38 0:b0ba4e08a18c 207 case 2 :
garfield38 0:b0ba4e08a18c 208 fix = FIX_2D;
garfield38 0:b0ba4e08a18c 209 break;
garfield38 0:b0ba4e08a18c 210 case 3 :
garfield38 0:b0ba4e08a18c 211 fix = FIX_3D;
garfield38 0:b0ba4e08a18c 212 break;
garfield38 0:b0ba4e08a18c 213 default:
garfield38 0:b0ba4e08a18c 214 fix = NO_FIX;
garfield38 0:b0ba4e08a18c 215 break;
garfield38 0:b0ba4e08a18c 216 }
garfield38 1:4eadcb718c8b 217 // change baud rate before sending akeru command
garfield38 0:b0ba4e08a18c 218 if((fix != NO_FIX) && (fixFlags & 0x02)) fix = FIX_DGPS;
garfield38 0:b0ba4e08a18c 219 }
garfield38 0:b0ba4e08a18c 220 // Decode compass data (not tilt compensated)
garfield38 0:b0ba4e08a18c 221 else if (msgId == NAZA_MESSAGE_COMPASS) {
garfield38 0:b0ba4e08a18c 222 uint8_t mask = gpsPayload[4];
garfield38 0:b0ba4e08a18c 223 mask = (((mask ^ (mask >> 4)) & 0x0F) | ((mask << 3) & 0xF0)) ^ (((mask & 0x01) << 3) | ((mask & 0x01) << 7));
garfield38 0:b0ba4e08a18c 224 int16_t x = decodeShort(0, mask);
garfield38 0:b0ba4e08a18c 225 int16_t y = decodeShort(2, mask);
garfield38 0:b0ba4e08a18c 226 if(x > magXMax) magXMax = x;
garfield38 0:b0ba4e08a18c 227 if(x < magXMin) magXMin = x;
garfield38 0:b0ba4e08a18c 228 if(y > magYMax) magYMax = y;
garfield38 0:b0ba4e08a18c 229 if(y < magYMin) magYMin = y;
garfield38 0:b0ba4e08a18c 230 headingNc = -atan2(y - ((magYMax + magYMin) / 2.0), x - ((magXMax + magXMin) / 2.0) ) * 180.0 / M_PI;
garfield38 0:b0ba4e08a18c 231 if(headingNc < 0) headingNc += 360.0;
garfield38 0:b0ba4e08a18c 232 }
garfield38 1:4eadcb718c8b 233 syncError = 0;
garfield38 0:b0ba4e08a18c 234 return msgId;
garfield38 0:b0ba4e08a18c 235 } else {
garfield38 0:b0ba4e08a18c 236 return NAZA_MESSAGE_NONE;
garfield38 0:b0ba4e08a18c 237 }
garfield38 0:b0ba4e08a18c 238 }