![](/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:
- 16:eb28d0f64a9b
- Child:
- 18:3f8a8f6e3cc1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Ublox6.cpp Thu Dec 13 17:35:29 2018 +0000 @@ -0,0 +1,188 @@ +#include "mbed.h" +#include "Ublox6.h" + +#define MAX_LENGTH 512 + +#define DOP_BIT 0x01 +#define POSLLH_BIT 0x02 +#define SOL_BIT 0x04 +#define VELNED_BIT 0x08 +#define SYNC1 0xB5 +#define SYNC2 0x62 +#define POSLLH_MSG 0x02 +#define SBAS_MSG 0x32 +#define VELNED_MSG 0x12 +#define STATUS_MSG 0x03 +#define SOL_MSG 0x06 +#define DOP_MSG 0x04 +#define DGPS_MSG 0x31 +#define SVINFO_MSG 0x30 +#define CFG 0x06 +#define NAV 0x01 +#define MSG 0x01 + +#define LONG(X) *(int32_t *)(&data[X]) +#define ULONG(X) *(uint32_t *)(&data[X]) +#define INT(X) *(int16_t *)(&data[X]) +#define UINT(X) *(uint16_t *)(&data[X]) + +// TODO 2: abstract LED/status to some kind of pub/sub thing + +void 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]; + + switch (state) { + case 0: // wait for sync 1 (0xB5) + ck1 = ck2 = 0; + checkOk = false; + if (cc == SYNC1) + state++; + break; + case 1: // wait for sync 2 (0x62) + if (cc == SYNC2) + state++; + else + state = 0; + break; + case 2: // wait for class code + code = cc; + ck1 += cc; + ck2 += ck1; + state++; + break; + case 3: // wait for Id + id = cc; + ck1 += cc; + ck2 += ck1; + state++; + break; + case 4: // wait for length uint8_t 1 + length = cc; + ck1 += cc; + ck2 += ck1; + state++; + break; + case 5: // wait for length uint8_t 2 + length |= (unsigned int) cc << 8; + ck1 += cc; + ck2 += ck1; + idx = 0; + state++; + if (length > MAX_LENGTH) + state= 0; + break; + case 6: // wait for <length> payload uint8_ts + data[idx++] = cc; + ck1 += cc; + ck2 += ck1; + if (idx >= length) { + state++; + } + break; + case 7: // wait for checksum 1 + chk1 = cc; + state++; + break; + case 8: { // wait for checksum 2 + chk2 = cc; + checkOk = ck1 == chk1 && ck2 == chk2; + if (!checkOk) { + // do something...? + } else { + switch (code) { + case 0x01: // NAV- + switch (id) { + case POSLLH_MSG: // NAV-POSLLH + _longitude = ((float)LONG(4))/10000000.0; + _latitude = ((float)LONG(8))/10000000.0; + // vAcc = ULONG(24); // mm + // hAcc = ULONG(20); // mm + _available |= 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; + _hdop = ((float) UINT(12))/100.0; + _available |= DOP_BIT; + break; + case SOL_MSG: // NAV-SOL + //week = UINT(8); + //pDOP = ((float) UINT(44))/ 100.0; + //pAcc = ULONG(24); + _sat_count = data[47]; + _available |= SOL_BIT; + break; + case VELNED_MSG: // NAV-VELNED + _speed_mps = ULONG(20)/100.0; + //sAcc = ULONG(28)/100.0; + _course_deg = ((float) LONG(24))/100000.0; + //cAcc = ((float) LONG(32))/100000.0; + _available |= VELNED_BIT; + break; + default: + break; + } + break; + case 0x05: // ACK- + switch (id) { + case 0x00: // ACK-NAK + break; + case 0x01: // ACK-ACK + break; + } + break; + } + } + state = 0; + break; + } + default: + break; + } +} + +bool Ublox6::available(void) +{ + return (_available & (DOP_BIT|POSLLH_BIT|SOL_BIT|VELNED_BIT)); +} + +void Ublox6::reset_available(void) +{ + _available = 0; +} + +double Ublox6::latitude(void) +{ + return _latitude; +} + +double Ublox6::longitude(void) +{ + return _longitude; +} + +float Ublox6::speed_mps(void) +{ + return _speed_mps; +} + +float Ublox6::heading_deg(void) +{ + return _course_deg; +} + +float Ublox6::hdop(void) +{ + return _hdop; +} + +int Ublox6::sat_count(void) +{ + return _sat_count; +}