![](/media/cache/profiles/2012_sf_avc_095.jpg.50x50_q85.jpg)
2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
Diff: Ublox6.cpp
- Revision:
- 19:0d1728091519
- Parent:
- 18:3f8a8f6e3cc1
- Child:
- 20:043987d06f8d
diff -r 3f8a8f6e3cc1 -r 0d1728091519 Ublox6.cpp --- a/Ublox6.cpp Fri Dec 14 00:36:06 2018 +0000 +++ b/Ublox6.cpp Sat Dec 15 21:38:11 2018 +0000 @@ -8,6 +8,8 @@ #define SOL_BIT 0x04 #define VELNED_BIT 0x08 +const int READY_BITS=POSLLH_BIT|DOP_BIT|SOL_BIT|VELNED_BIT; + #define SYNC1 0xB5 #define SYNC2 0x62 @@ -44,26 +46,32 @@ latest = tmp; } -void Ublox6::parse(unsigned char cc) +int Ublox6::parse(unsigned char cc) { //unsigned char cc = buf[out++]; //out &= (MAX_LENGTH-1); static unsigned char ck1, ck2, state, code, id, idx, length, chk1, chk2; static bool checkOk; static unsigned char data[MAX_LENGTH]; + int status = 0; switch (state) { case 0: // wait for sync 1 (0xB5) ck1 = ck2 = 0; checkOk = false; - if (cc == SYNC1) + if (cc == SYNC1) { state++; + puts("SYNC1"); + } break; case 1: // wait for sync 2 (0x62) - if (cc == SYNC2) + if (cc == SYNC2) { state++; - else + puts("SYNC2"); + } else { state = 0; + puts("NO SYNC2"); + } break; case 2: // wait for class code code = cc; @@ -85,6 +93,7 @@ break; case 5: // wait for length uint8_t 2 length |= (unsigned int) cc << 8; + printf("len=%d\n", length); ck1 += cc; ck2 += ck1; idx = 0; @@ -108,51 +117,58 @@ chk2 = cc; checkOk = ck1 == chk1 && ck2 == chk2; if (!checkOk) { - // do something...? + puts("BAD CHKSUM\n"); } else { switch (code) { - case 0x01: // NAV- - switch (id) { - case POSLLH_MSG: // NAV-POSLLH - tmp.lon = ((float)LONG(4))/10000000.0; - tmp.lat = ((float)LONG(8))/10000000.0; - // vAcc = ULONG(24); // mm - // hAcc = ULONG(20); // mm - _ready |= POSLLH_BIT; - break; - case DOP_MSG: // NAV-DOP - //gDOP = ((float) UINT(4))/100.0; - //tDOP = ((float) UINT(8))/100.0; - //vDOP = ((float) UINT(10))/100.0; - tmp.hdop = ((float) UINT(12))/100.0; - _ready |= DOP_BIT; - break; - case SOL_MSG: // NAV-SOL - //week = UINT(8); - //pDOP = ((float) UINT(44))/ 100.0; - //pAcc = ULONG(24); - tmp.svcount = data[47]; - _ready |= SOL_BIT; - break; - case VELNED_MSG: // NAV-VELNED - tmp.speed = ULONG(20)/100.0; - //sAcc = ULONG(28)/100.0; - tmp.course = ((float) LONG(24))/100000.0; - //cAcc = ((float) LONG(32))/100000.0; - _ready |= VELNED_BIT; - break; - default: - break; - } + case 0x01: // NAV- + switch (id) { + case POSLLH_MSG: // NAV-POSLLH + tmp.lon = ((float)LONG(4))/10000000.0; + tmp.lat = ((float)LONG(8))/10000000.0; + // vAcc = ULONG(24); // mm + // hAcc = ULONG(20); // mm + _ready |= POSLLH_BIT; + puts("LAT/LON"); + break; + case DOP_MSG: // NAV-DOP + //gDOP = ((float) UINT(4))/100.0; + //tDOP = ((float) UINT(8))/100.0; + //vDOP = ((float) UINT(10))/100.0; + tmp.hdop = ((float) UINT(12))/100.0; + puts("HDOP"); + _ready |= DOP_BIT; break; - case 0x05: // ACK- - switch (id) { - case 0x00: // ACK-NAK - break; - case 0x01: // ACK-ACK - break; - } + case SOL_MSG: // NAV-SOL + //week = UINT(8); + //pDOP = ((float) UINT(44))/ 100.0; + //pAcc = ULONG(24); + tmp.svcount = data[47]; + _ready |= SOL_BIT; + puts("SVCOUNT"); + break; + case VELNED_MSG: // NAV-VELNED + tmp.speed = ULONG(20)/100.0; + //sAcc = ULONG(28)/100.0; + tmp.course = ((float) LONG(24))/100000.0; + //cAcc = ((float) LONG(32))/100000.0; + _ready |= VELNED_BIT; + puts("SPD/COURSE"); break; + default: + printf("OTHER=0x%04x\n", id); + break; + } + if (_ready) + printf("_ready = 0x%04x 0x%04x\n", _ready, READY_BITS); + break; + case 0x05: // ACK- + switch (id) { + case 0x00: // ACK-NAK + break; + case 0x01: // ACK-ACK + break; + } + break; } } state = 0; @@ -161,14 +177,21 @@ default: break; } - - if (_ready == POSLLH_BIT|DOP_BIT|SOL_BIT|VELNED_BIT) { + + if ((_ready & READY_BITS) == READY_BITS) { latest = tmp; + tmp.lat = 0; + tmp.lon = 0; + tmp.speed = 0; + tmp.course = 0; + tmp.hdop = 0; + tmp.svcount = 0; _ready = 0; + status = 1; _available = true; } - return; + return status; } void Ublox6::read(double& lat, double& lon, float& course, float& speed, float& hdop, int& svcount) { @@ -182,14 +205,12 @@ return; } - - +/* bool Ublox6::available(void) { return _available; } -/* double Ublox6::latitude(void) { return _latitude;