2018 revision to classic DataBus AVC code.
Dependencies: LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell
Diff: GPS/Ublox6.cpp
- Revision:
- 36:3095e00eef37
- Parent:
- 24:a7f92dfc5310
diff -r c42e7e29c3bd -r 3095e00eef37 GPS/Ublox6.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/GPS/Ublox6.cpp Tue Jan 01 17:44:27 2019 +0000 @@ -0,0 +1,241 @@ +#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 + +const int READY_BITS=POSLLH_BIT|DOP_BIT|SOL_BIT|VELNED_BIT; + +#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]) + +Ublox6::Ublox6() +{ + // clear flags + _ready = 0; + _available = false; + // clear out structs + tmp.lat = 0; + tmp.lon = 0; + tmp.course = 0; + tmp.speed = 0; + tmp.hdop = 0; + tmp.svcount = 0; + latest = tmp; + _callback = NULL; +} + +int Ublox6::parse(int 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) { + 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) { + 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; + } + break; + case 0x05: // ACK- + switch (id) { + case 0x00: // ACK-NAK + break; + case 0x01: // ACK-ACK + break; + } + break; + } + } + state = 0; + break; + } + default: + break; + } + + if ((_ready & READY_BITS) == READY_BITS) { + latest = tmp; + _ready = 0; + status = 1; + + // clear tmp + tmp.lat = 0; + tmp.lon = 0; + tmp.speed = 0; + tmp.course = 0; + tmp.hdop = 0; + tmp.svcount = 0; + if (_callback) + _callback(); + } + + return status; +} + + +void Ublox6::read(double& lat, double& lon, float& course, float& speed, float& hdop, int& svcount) { + lat = latest.lat; + lon = latest.lon; + course = latest.course; + speed = latest.speed; + hdop = latest.hdop; + svcount = latest.svcount; + + return; +} + + +void Ublox6::subscribe(Callback<void()> cb) { + _callback = cb; + + return; +} + +/* +bool Ublox6::available(void) +{ + return _available; +} + +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 _svcount; +} +*/