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@0:b0ba4e08a18c, 2014-11-18 (annotated)

Committer:
garfield38
Date:
Tue Nov 18 16:39:57 2014 +0000
Revision:
0:b0ba4e08a18c
Child:
1:4eadcb718c8b
first release

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