uint8_t NazaDecoderLib::decode(int input)
{
if((seq == 0) && (input == 0x55)) { seq++; } // header (part 1 - 0x55)
else if((seq == 1) && (input == 0xAA)) { cs1 = 0; cs2 = 0; seq++; } // header (part 2 - 0xAA)
else if((seq == 2) && ((input == 0x10) || (input == 0x20))) { msgId = input; updateCS(input); seq++; } // message id
else if(seq == 3) { msgLen = input; cnt = 0; updateCS(input); seq++; } // message payload lenght
else if(seq == 4) { gpsPayload[cnt++] = input; updateCS(input); if(cnt >= msgLen) { seq++; } } // store payload in buffer
else if((seq == 5) && (input == cs1)) { seq++; } // verify checksum #1
else if((seq == 6) && (input == cs2)) { seq++; } // verify checksum #2
else seq = 0;
if(seq == 7) // all data in buffer
{
seq = 0;
// Decode GPS data
if(msgId == NAZA_MESSAGE_GPS)
{
uint8_t mask = gpsPayload[55];
uint32_t time = decodeLong(0, mask);
second = time & 0b00111111; time >>= 6;
minute = time & 0b00111111; time >>= 6;
hour = time & 0b00001111; time >>= 4;
day = time & 0b00011111; time >>= 5; if(hour > 7) day++;
month = time & 0b00001111; time >>= 4;
year = time & 0b01111111;
lon = (uint32_t)decodeLong(4, mask) / 1;
lat = (uint32_t)decodeLong(8, mask) / 1;
gpsAlt = (double)decodeLong(12, mask) / 1000;
double nVel = (double)decodeLong(28, mask) / 100;
double eVel = (double)decodeLong(32, mask) / 100;
spd = sqrt(nVel * nVel + eVel * eVel);
cog = atan2(eVel, nVel) * 180.0 / M_PI;
if(cog < 0) cog += 360.0;
gpsVsi = -(double)decodeLong(36, mask) / 100;
vdop = (double)decodeShort(42, mask) / 100;
double ndop = (double)decodeShort(44, mask) / 100;
double edop = (double)decodeShort(46, mask) / 100;
hdop = sqrt(ndop * ndop + edop * edop);
sat = gpsPayload[48];
uint8_t fixType = gpsPayload[50] ^ mask;
uint8_t fixFlags = gpsPayload[52] ^ mask;
switch(fixType)
{
case 2 : fix = FIX_2D; break;
case 3 : fix = FIX_3D; break;
default: fix = NO_FIX; break;
}
if((fix != NO_FIX) && (fixFlags & 0x02)) fix = FIX_DGPS;
}
int ObjectBlueprint::decodeInt( const char* data,
int* idx,
boost::any* output )
{
return decodeLong(data, idx, output);
}
