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@2:de84f8a0a706, 2014-11-28 (annotated)

Committer:
garfield38
Date:
Fri Nov 28 17:50:46 2014 +0000
Revision:
2:de84f8a0a706
Parent:
1:4eadcb718c8b 
Added comments

Who changed what in which revision?

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