![](/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
Ublox6.cpp
- Committer:
- shimniok
- Date:
- 2018-12-15
- Revision:
- 19:0d1728091519
- Parent:
- 18:3f8a8f6e3cc1
- Child:
- 20:043987d06f8d
File content as of revision 19:0d1728091519:
#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; } 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) { state++; puts("SYNC1"); } break; case 1: // wait for sync 2 (0x62) if (cc == SYNC2) { state++; puts("SYNC2"); } else { state = 0; puts("NO SYNC2"); } 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; printf("len=%d\n", length); 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) { 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; 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 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; break; } default: break; } 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 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; } /* 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; } */