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