http://www.rcgroups.com/forums/showthread.php?t=1995704
Use the following image to get GPS signal and supply:
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?
User | Revision | Line number | New 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 | } |