http://www.rcgroups.com/forums/showthread.php?t=1995704
Use the following image to get GPS signal and supply:
Diff: NazaDecoderLib.cpp
- Revision:
- 1:4eadcb718c8b
- Parent:
- 0:b0ba4e08a18c
- Child:
- 2:de84f8a0a706
diff -r b0ba4e08a18c -r 4eadcb718c8b NazaDecoderLib.cpp --- a/NazaDecoderLib.cpp Tue Nov 18 16:39:57 2014 +0000 +++ b/NazaDecoderLib.cpp Fri Nov 28 17:39:38 2014 +0000 @@ -4,17 +4,20 @@ Not for commercial use Refer to naza_decoder_wiring.jpg diagram for proper connection - - The RC PWM input code taken from https://www.instructables.com/id/RC-Quadrotor-Helicopter/step12/Arduino-Demo-PWM-Input/ + http://www.rcgroups.com/forums/showthread.php?t=1995704 + */ //#include "Arduino.h" +#include "mbed.h" + #include "NazaDecoderLib.h" #include "stdint.h" #include "math.h" #define M_PI 3.14159265358979323846 NazaDecoderLib NazaDecoder; +Timer tn; NazaDecoderLib::NazaDecoderLib() { @@ -24,16 +27,17 @@ msgLen = 0; cs1 = 0; cs2 = 0; + syncError = 0; } int32_t NazaDecoderLib::decodeLong(uint8_t idx, uint8_t mask) { uint32_t variableToHoldValue = 0; unsigned char *pBytePointer; //declare a byte-pointer - + // I suppose little-endian pBytePointer = (unsigned char *)&variableToHoldValue; for(int i = 0; i < 4; i++) { - *(pBytePointer+i) = gpsPayload[idx + i] ^ mask; + *(pBytePointer+i) = gpsPayload[idx + i] ^ mask; } return variableToHoldValue; } @@ -42,7 +46,7 @@ { uint16_t variableToHoldValue = 0; unsigned char *pBytePointer; //declare a byte-pointer - + pBytePointer = (unsigned char *)&variableToHoldValue; for(int i = 0; i < 2; i++) { *(pBytePointer+i) = gpsPayload[idx + i] ^ mask; @@ -50,12 +54,20 @@ return variableToHoldValue; } -void NazaDecoderLib::updateCS(int input) +void NazaDecoderLib::updateCS(uint8_t input) { cs1 += input; cs2 += cs1; } +void NazaDecoderLib::getDebug(RawSerial &s, char* buf) +{ + 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",\ + headingNc, lat, lon, gpsAlt, sat, year, month, day, hour, minute,\ + second, tn.read_ms(), syncError); + s.puts(buf); +} + double NazaDecoderLib::getLat() { return lat; @@ -125,9 +137,10 @@ return second; } -uint8_t NazaDecoderLib::decode(int input) +uint8_t NazaDecoderLib::decode(uint8_t input) { if((seq == 0) && (input == 0x55)) { + tn.reset(); seq++; // header (part 1 - 0x55) } else if((seq == 1) && (input == 0xAA)) { cs1 = 0; // header (part 2 - 0xAA) @@ -152,12 +165,14 @@ seq++; // verify checksum #1 } else if((seq == 6) && (input == cs2)) { seq++; // verify checksum #2 - } else seq = 0; - + } else { + tn.start(); + seq = 0; + syncError++; + } if(seq == 7) { // all data in buffer seq = 0; - // Decode GPS data - if(msgId == NAZA_MESSAGE_GPS) { + if(msgId == NAZA_MESSAGE_GPS) { // Decode GPS data uint8_t mask = gpsPayload[55]; uint32_t time = decodeLong(0, mask); second = time & 0x3f; @@ -199,6 +214,7 @@ fix = NO_FIX; break; } + // change baud rate before sending akeru command if((fix != NO_FIX) && (fixFlags & 0x02)) fix = FIX_DGPS; } // Decode compass data (not tilt compensated) @@ -214,6 +230,7 @@ headingNc = -atan2(y - ((magYMax + magYMin) / 2.0), x - ((magXMax + magXMin) / 2.0) ) * 180.0 / M_PI; if(headingNc < 0) headingNc += 360.0; } + syncError = 0; return msgId; } else { return NAZA_MESSAGE_NONE;