Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

Ublox6.cpp@16:eb28d0f64a9b, 2018-12-13 (annotated)

Committer:
shimniok
Date:
Thu Dec 13 17:35:29 2018 +0000
Revision:
16:eb28d0f64a9b
Child:
18:3f8a8f6e3cc1
Initial gps receive implementation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 16:eb28d0f64a9b 1 #include "mbed.h"
shimniok 16:eb28d0f64a9b 2 #include "Ublox6.h"
shimniok 16:eb28d0f64a9b 3
shimniok 16:eb28d0f64a9b 4 #define MAX_LENGTH 512
shimniok 16:eb28d0f64a9b 5
shimniok 16:eb28d0f64a9b 6 #define DOP_BIT 0x01
shimniok 16:eb28d0f64a9b 7 #define POSLLH_BIT 0x02
shimniok 16:eb28d0f64a9b 8 #define SOL_BIT 0x04
shimniok 16:eb28d0f64a9b 9 #define VELNED_BIT 0x08
shimniok 16:eb28d0f64a9b 10 #define SYNC1 0xB5
shimniok 16:eb28d0f64a9b 11 #define SYNC2 0x62
shimniok 16:eb28d0f64a9b 12 #define POSLLH_MSG 0x02
shimniok 16:eb28d0f64a9b 13 #define SBAS_MSG 0x32
shimniok 16:eb28d0f64a9b 14 #define VELNED_MSG 0x12
shimniok 16:eb28d0f64a9b 15 #define STATUS_MSG 0x03
shimniok 16:eb28d0f64a9b 16 #define SOL_MSG 0x06
shimniok 16:eb28d0f64a9b 17 #define DOP_MSG 0x04
shimniok 16:eb28d0f64a9b 18 #define DGPS_MSG 0x31
shimniok 16:eb28d0f64a9b 19 #define SVINFO_MSG 0x30
shimniok 16:eb28d0f64a9b 20 #define CFG 0x06
shimniok 16:eb28d0f64a9b 21 #define NAV 0x01
shimniok 16:eb28d0f64a9b 22 #define MSG 0x01
shimniok 16:eb28d0f64a9b 23
shimniok 16:eb28d0f64a9b 24 #define LONG(X) *(int32_t *)(&data[X])
shimniok 16:eb28d0f64a9b 25 #define ULONG(X) *(uint32_t *)(&data[X])
shimniok 16:eb28d0f64a9b 26 #define INT(X) *(int16_t *)(&data[X])
shimniok 16:eb28d0f64a9b 27 #define UINT(X) *(uint16_t *)(&data[X])
shimniok 16:eb28d0f64a9b 28
shimniok 16:eb28d0f64a9b 29 // TODO 2: abstract LED/status to some kind of pub/sub thing
shimniok 16:eb28d0f64a9b 30
shimniok 16:eb28d0f64a9b 31 void Ublox6::parse(unsigned char cc)
shimniok 16:eb28d0f64a9b 32 {
shimniok 16:eb28d0f64a9b 33 //unsigned char cc = buf[out++];
shimniok 16:eb28d0f64a9b 34 //out &= (MAX_LENGTH-1);
shimniok 16:eb28d0f64a9b 35 static unsigned char ck1, ck2, state, code, id, idx, length, chk1, chk2;
shimniok 16:eb28d0f64a9b 36 static bool checkOk;
shimniok 16:eb28d0f64a9b 37 static unsigned char data[MAX_LENGTH];
shimniok 16:eb28d0f64a9b 38
shimniok 16:eb28d0f64a9b 39 switch (state) {
shimniok 16:eb28d0f64a9b 40 case 0: // wait for sync 1 (0xB5)
shimniok 16:eb28d0f64a9b 41 ck1 = ck2 = 0;
shimniok 16:eb28d0f64a9b 42 checkOk = false;
shimniok 16:eb28d0f64a9b 43 if (cc == SYNC1)
shimniok 16:eb28d0f64a9b 44 state++;
shimniok 16:eb28d0f64a9b 45 break;
shimniok 16:eb28d0f64a9b 46 case 1: // wait for sync 2 (0x62)
shimniok 16:eb28d0f64a9b 47 if (cc == SYNC2)
shimniok 16:eb28d0f64a9b 48 state++;
shimniok 16:eb28d0f64a9b 49 else
shimniok 16:eb28d0f64a9b 50 state = 0;
shimniok 16:eb28d0f64a9b 51 break;
shimniok 16:eb28d0f64a9b 52 case 2: // wait for class code
shimniok 16:eb28d0f64a9b 53 code = cc;
shimniok 16:eb28d0f64a9b 54 ck1 += cc;
shimniok 16:eb28d0f64a9b 55 ck2 += ck1;
shimniok 16:eb28d0f64a9b 56 state++;
shimniok 16:eb28d0f64a9b 57 break;
shimniok 16:eb28d0f64a9b 58 case 3: // wait for Id
shimniok 16:eb28d0f64a9b 59 id = cc;
shimniok 16:eb28d0f64a9b 60 ck1 += cc;
shimniok 16:eb28d0f64a9b 61 ck2 += ck1;
shimniok 16:eb28d0f64a9b 62 state++;
shimniok 16:eb28d0f64a9b 63 break;
shimniok 16:eb28d0f64a9b 64 case 4: // wait for length uint8_t 1
shimniok 16:eb28d0f64a9b 65 length = cc;
shimniok 16:eb28d0f64a9b 66 ck1 += cc;
shimniok 16:eb28d0f64a9b 67 ck2 += ck1;
shimniok 16:eb28d0f64a9b 68 state++;
shimniok 16:eb28d0f64a9b 69 break;
shimniok 16:eb28d0f64a9b 70 case 5: // wait for length uint8_t 2
shimniok 16:eb28d0f64a9b 71 length |= (unsigned int) cc << 8;
shimniok 16:eb28d0f64a9b 72 ck1 += cc;
shimniok 16:eb28d0f64a9b 73 ck2 += ck1;
shimniok 16:eb28d0f64a9b 74 idx = 0;
shimniok 16:eb28d0f64a9b 75 state++;
shimniok 16:eb28d0f64a9b 76 if (length > MAX_LENGTH)
shimniok 16:eb28d0f64a9b 77 state= 0;
shimniok 16:eb28d0f64a9b 78 break;
shimniok 16:eb28d0f64a9b 79 case 6: // wait for <length> payload uint8_ts
shimniok 16:eb28d0f64a9b 80 data[idx++] = cc;
shimniok 16:eb28d0f64a9b 81 ck1 += cc;
shimniok 16:eb28d0f64a9b 82 ck2 += ck1;
shimniok 16:eb28d0f64a9b 83 if (idx >= length) {
shimniok 16:eb28d0f64a9b 84 state++;
shimniok 16:eb28d0f64a9b 85 }
shimniok 16:eb28d0f64a9b 86 break;
shimniok 16:eb28d0f64a9b 87 case 7: // wait for checksum 1
shimniok 16:eb28d0f64a9b 88 chk1 = cc;
shimniok 16:eb28d0f64a9b 89 state++;
shimniok 16:eb28d0f64a9b 90 break;
shimniok 16:eb28d0f64a9b 91 case 8: { // wait for checksum 2
shimniok 16:eb28d0f64a9b 92 chk2 = cc;
shimniok 16:eb28d0f64a9b 93 checkOk = ck1 == chk1 && ck2 == chk2;
shimniok 16:eb28d0f64a9b 94 if (!checkOk) {
shimniok 16:eb28d0f64a9b 95 // do something...?
shimniok 16:eb28d0f64a9b 96 } else {
shimniok 16:eb28d0f64a9b 97 switch (code) {
shimniok 16:eb28d0f64a9b 98 case 0x01: // NAV-
shimniok 16:eb28d0f64a9b 99 switch (id) {
shimniok 16:eb28d0f64a9b 100 case POSLLH_MSG: // NAV-POSLLH
shimniok 16:eb28d0f64a9b 101 _longitude = ((float)LONG(4))/10000000.0;
shimniok 16:eb28d0f64a9b 102 _latitude = ((float)LONG(8))/10000000.0;
shimniok 16:eb28d0f64a9b 103 // vAcc = ULONG(24); // mm
shimniok 16:eb28d0f64a9b 104 // hAcc = ULONG(20); // mm
shimniok 16:eb28d0f64a9b 105 _available |= POSLLH_BIT;
shimniok 16:eb28d0f64a9b 106 break;
shimniok 16:eb28d0f64a9b 107 case DOP_MSG: // NAV-DOP
shimniok 16:eb28d0f64a9b 108 //gDOP = ((float) UINT(4))/100.0;
shimniok 16:eb28d0f64a9b 109 //tDOP = ((float) UINT(8))/100.0;
shimniok 16:eb28d0f64a9b 110 //vDOP = ((float) UINT(10))/100.0;
shimniok 16:eb28d0f64a9b 111 _hdop = ((float) UINT(12))/100.0;
shimniok 16:eb28d0f64a9b 112 _available |= DOP_BIT;
shimniok 16:eb28d0f64a9b 113 break;
shimniok 16:eb28d0f64a9b 114 case SOL_MSG: // NAV-SOL
shimniok 16:eb28d0f64a9b 115 //week = UINT(8);
shimniok 16:eb28d0f64a9b 116 //pDOP = ((float) UINT(44))/ 100.0;
shimniok 16:eb28d0f64a9b 117 //pAcc = ULONG(24);
shimniok 16:eb28d0f64a9b 118 _sat_count = data[47];
shimniok 16:eb28d0f64a9b 119 _available |= SOL_BIT;
shimniok 16:eb28d0f64a9b 120 break;
shimniok 16:eb28d0f64a9b 121 case VELNED_MSG: // NAV-VELNED
shimniok 16:eb28d0f64a9b 122 _speed_mps = ULONG(20)/100.0;
shimniok 16:eb28d0f64a9b 123 //sAcc = ULONG(28)/100.0;
shimniok 16:eb28d0f64a9b 124 _course_deg = ((float) LONG(24))/100000.0;
shimniok 16:eb28d0f64a9b 125 //cAcc = ((float) LONG(32))/100000.0;
shimniok 16:eb28d0f64a9b 126 _available |= VELNED_BIT;
shimniok 16:eb28d0f64a9b 127 break;
shimniok 16:eb28d0f64a9b 128 default:
shimniok 16:eb28d0f64a9b 129 break;
shimniok 16:eb28d0f64a9b 130 }
shimniok 16:eb28d0f64a9b 131 break;
shimniok 16:eb28d0f64a9b 132 case 0x05: // ACK-
shimniok 16:eb28d0f64a9b 133 switch (id) {
shimniok 16:eb28d0f64a9b 134 case 0x00: // ACK-NAK
shimniok 16:eb28d0f64a9b 135 break;
shimniok 16:eb28d0f64a9b 136 case 0x01: // ACK-ACK
shimniok 16:eb28d0f64a9b 137 break;
shimniok 16:eb28d0f64a9b 138 }
shimniok 16:eb28d0f64a9b 139 break;
shimniok 16:eb28d0f64a9b 140 }
shimniok 16:eb28d0f64a9b 141 }
shimniok 16:eb28d0f64a9b 142 state = 0;
shimniok 16:eb28d0f64a9b 143 break;
shimniok 16:eb28d0f64a9b 144 }
shimniok 16:eb28d0f64a9b 145 default:
shimniok 16:eb28d0f64a9b 146 break;
shimniok 16:eb28d0f64a9b 147 }
shimniok 16:eb28d0f64a9b 148 }
shimniok 16:eb28d0f64a9b 149
shimniok 16:eb28d0f64a9b 150 bool Ublox6::available(void)
shimniok 16:eb28d0f64a9b 151 {
shimniok 16:eb28d0f64a9b 152 return (_available & (DOP_BIT|POSLLH_BIT|SOL_BIT|VELNED_BIT));
shimniok 16:eb28d0f64a9b 153 }
shimniok 16:eb28d0f64a9b 154
shimniok 16:eb28d0f64a9b 155 void Ublox6::reset_available(void)
shimniok 16:eb28d0f64a9b 156 {
shimniok 16:eb28d0f64a9b 157 _available = 0;
shimniok 16:eb28d0f64a9b 158 }
shimniok 16:eb28d0f64a9b 159
shimniok 16:eb28d0f64a9b 160 double Ublox6::latitude(void)
shimniok 16:eb28d0f64a9b 161 {
shimniok 16:eb28d0f64a9b 162 return _latitude;
shimniok 16:eb28d0f64a9b 163 }
shimniok 16:eb28d0f64a9b 164
shimniok 16:eb28d0f64a9b 165 double Ublox6::longitude(void)
shimniok 16:eb28d0f64a9b 166 {
shimniok 16:eb28d0f64a9b 167 return _longitude;
shimniok 16:eb28d0f64a9b 168 }
shimniok 16:eb28d0f64a9b 169
shimniok 16:eb28d0f64a9b 170 float Ublox6::speed_mps(void)
shimniok 16:eb28d0f64a9b 171 {
shimniok 16:eb28d0f64a9b 172 return _speed_mps;
shimniok 16:eb28d0f64a9b 173 }
shimniok 16:eb28d0f64a9b 174
shimniok 16:eb28d0f64a9b 175 float Ublox6::heading_deg(void)
shimniok 16:eb28d0f64a9b 176 {
shimniok 16:eb28d0f64a9b 177 return _course_deg;
shimniok 16:eb28d0f64a9b 178 }
shimniok 16:eb28d0f64a9b 179
shimniok 16:eb28d0f64a9b 180 float Ublox6::hdop(void)
shimniok 16:eb28d0f64a9b 181 {
shimniok 16:eb28d0f64a9b 182 return _hdop;
shimniok 16:eb28d0f64a9b 183 }
shimniok 16:eb28d0f64a9b 184
shimniok 16:eb28d0f64a9b 185 int Ublox6::sat_count(void)
shimniok 16:eb28d0f64a9b 186 {
shimniok 16:eb28d0f64a9b 187 return _sat_count;
shimniok 16:eb28d0f64a9b 188 }