iforce2d Chris / Mbed 2 deprecated ubxDistanceMeter

Displays distance to start location on OLED screen.

Dependencies:   mbed

main.cpp@0:972874f31c98, 2018-03-07 (annotated)

Committer:
iforce2d
Date:
Wed Mar 07 12:49:14 2018 +0000
Revision:
0:972874f31c98
First commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
iforce2d 0:972874f31c98 1 /*
iforce2d 0:972874f31c98 2 Reads UBX NAV-PVT messages from a u-blox GPS module and displays the distance in
iforce2d 0:972874f31c98 3 centimeters between the current location and the first location received at power
iforce2d 0:972874f31c98 4 up. No configuration messages are sent to the GPS so it needs to be prepared to
iforce2d 0:972874f31c98 5 output NAV-PVT messages ONLY, and have that configuration saved so it will always
iforce2d 0:972874f31c98 6 do so when powered up.
iforce2d 0:972874f31c98 7 Screen is a SSD1306 running on I2C connection.
iforce2d 0:972874f31c98 8 */
iforce2d 0:972874f31c98 9 #include "mbed.h"
iforce2d 0:972874f31c98 10 #include "Clock.h"
iforce2d 0:972874f31c98 11 #include "u8g.h"
iforce2d 0:972874f31c98 12
iforce2d 0:972874f31c98 13 #define OLED_ADDRESS 0x78
iforce2d 0:972874f31c98 14
iforce2d 0:972874f31c98 15 DigitalOut myled(PB_1);
iforce2d 0:972874f31c98 16 I2C i2c(PB_7, PB_6); // sda, scl
iforce2d 0:972874f31c98 17 Serial gps(PA_2, PA_3, 19200); // tx, rx
iforce2d 0:972874f31c98 18
iforce2d 0:972874f31c98 19 // At 19200 baud the buffer gets overwritten too quickly. We need to make our
iforce2d 0:972874f31c98 20 // own software buffer and update it by interrupts so we don't miss anything.
iforce2d 0:972874f31c98 21 void Rx_interrupt();
iforce2d 0:972874f31c98 22 const int buffer_size = 1024;
iforce2d 0:972874f31c98 23 char rx_buffer[buffer_size+1];
iforce2d 0:972874f31c98 24 volatile int rx_in=0;
iforce2d 0:972874f31c98 25 volatile int rx_out=0;
iforce2d 0:972874f31c98 26
iforce2d 0:972874f31c98 27
iforce2d 0:972874f31c98 28 void u8g_Delay(uint16_t usec)
iforce2d 0:972874f31c98 29 {
iforce2d 0:972874f31c98 30 wait_us((int)usec);
iforce2d 0:972874f31c98 31 }
iforce2d 0:972874f31c98 32
iforce2d 0:972874f31c98 33 void u8g_MicroDelay()
iforce2d 0:972874f31c98 34 {
iforce2d 0:972874f31c98 35 wait_us(1);
iforce2d 0:972874f31c98 36 }
iforce2d 0:972874f31c98 37
iforce2d 0:972874f31c98 38 void u8g_10MicroDelay()
iforce2d 0:972874f31c98 39 {
iforce2d 0:972874f31c98 40 wait_us(10);
iforce2d 0:972874f31c98 41 }
iforce2d 0:972874f31c98 42
iforce2d 0:972874f31c98 43 uint8_t u8g_com_hw_i2c_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr)
iforce2d 0:972874f31c98 44 {
iforce2d 0:972874f31c98 45 static uint8_t control = 0;
iforce2d 0:972874f31c98 46
iforce2d 0:972874f31c98 47 switch(msg) {
iforce2d 0:972874f31c98 48 case U8G_COM_MSG_STOP:
iforce2d 0:972874f31c98 49 break; //do nothing...
iforce2d 0:972874f31c98 50
iforce2d 0:972874f31c98 51 case U8G_COM_MSG_INIT:
iforce2d 0:972874f31c98 52 u8g_MicroDelay();
iforce2d 0:972874f31c98 53 break;
iforce2d 0:972874f31c98 54
iforce2d 0:972874f31c98 55 case U8G_COM_MSG_ADDRESS:
iforce2d 0:972874f31c98 56 //the control byte switches the mode on the device and is set here
iforce2d 0:972874f31c98 57 u8g_10MicroDelay();
iforce2d 0:972874f31c98 58 if (arg_val == 0) {
iforce2d 0:972874f31c98 59 control = 0;
iforce2d 0:972874f31c98 60 } else {
iforce2d 0:972874f31c98 61 control = 0x40;
iforce2d 0:972874f31c98 62 }
iforce2d 0:972874f31c98 63 break;
iforce2d 0:972874f31c98 64
iforce2d 0:972874f31c98 65 case U8G_COM_MSG_WRITE_BYTE: {
iforce2d 0:972874f31c98 66 //simple: just write one byte
iforce2d 0:972874f31c98 67 char buffer[2];
iforce2d 0:972874f31c98 68 buffer[0] = control;
iforce2d 0:972874f31c98 69 buffer[1] = arg_val;
iforce2d 0:972874f31c98 70 i2c.write(OLED_ADDRESS, buffer, 2);
iforce2d 0:972874f31c98 71 }
iforce2d 0:972874f31c98 72 break;
iforce2d 0:972874f31c98 73
iforce2d 0:972874f31c98 74 case U8G_COM_MSG_WRITE_SEQ:
iforce2d 0:972874f31c98 75 case U8G_COM_MSG_WRITE_SEQ_P: {
iforce2d 0:972874f31c98 76 char buffer[1024];
iforce2d 0:972874f31c98 77 char *ptr = (char*)arg_ptr;
iforce2d 0:972874f31c98 78 buffer[0] = control;
iforce2d 0:972874f31c98 79 for (int i = 1; i <= arg_val; i++) {
iforce2d 0:972874f31c98 80 buffer[i] = *(ptr++);
iforce2d 0:972874f31c98 81 }
iforce2d 0:972874f31c98 82 i2c.write(OLED_ADDRESS, buffer, arg_val);
iforce2d 0:972874f31c98 83 }
iforce2d 0:972874f31c98 84 break;
iforce2d 0:972874f31c98 85
iforce2d 0:972874f31c98 86 }
iforce2d 0:972874f31c98 87 return 1;
iforce2d 0:972874f31c98 88 }
iforce2d 0:972874f31c98 89
iforce2d 0:972874f31c98 90 #ifndef DEGTORAD
iforce2d 0:972874f31c98 91 #define DEGTORAD 0.0174532925199432957f
iforce2d 0:972874f31c98 92 #define RADTODEG 57.295779513082320876f
iforce2d 0:972874f31c98 93 #endif
iforce2d 0:972874f31c98 94
iforce2d 0:972874f31c98 95 struct geoloc {
iforce2d 0:972874f31c98 96 double lat;
iforce2d 0:972874f31c98 97 double lon;
iforce2d 0:972874f31c98 98 geoloc(double x = 0, double y = 0, int32_t a = 0) {
iforce2d 0:972874f31c98 99 lat = x;
iforce2d 0:972874f31c98 100 lon = y;
iforce2d 0:972874f31c98 101 }
iforce2d 0:972874f31c98 102 };
iforce2d 0:972874f31c98 103
iforce2d 0:972874f31c98 104 // http://www.movable-type.co.uk/scripts/latlong.html
iforce2d 0:972874f31c98 105 float geoDistance(struct geoloc &a, struct geoloc &b) // returns meters
iforce2d 0:972874f31c98 106 {
iforce2d 0:972874f31c98 107 const double R = 6371000; // km
iforce2d 0:972874f31c98 108 double p1 = a.lat * DEGTORAD;
iforce2d 0:972874f31c98 109 double p2 = b.lat * DEGTORAD;
iforce2d 0:972874f31c98 110 double dp = (b.lat-a.lat) * DEGTORAD;
iforce2d 0:972874f31c98 111 double dl = (b.lon-a.lon) * DEGTORAD;
iforce2d 0:972874f31c98 112
iforce2d 0:972874f31c98 113 double x = sin(dp/2) * sin(dp/2) + cos(p1) * cos(p2) * sin(dl/2) * sin(dl/2);
iforce2d 0:972874f31c98 114 double y = 2 * atan2(sqrt(x), sqrt(1-x));
iforce2d 0:972874f31c98 115
iforce2d 0:972874f31c98 116 return R * y;
iforce2d 0:972874f31c98 117 }
iforce2d 0:972874f31c98 118
iforce2d 0:972874f31c98 119 geoloc originLocation;
iforce2d 0:972874f31c98 120 geoloc currentLocation;
iforce2d 0:972874f31c98 121
iforce2d 0:972874f31c98 122 const unsigned char UBX_HEADER[] = { 0xB5, 0x62 };
iforce2d 0:972874f31c98 123
iforce2d 0:972874f31c98 124 struct NAV_PVT {
iforce2d 0:972874f31c98 125 unsigned char cls;
iforce2d 0:972874f31c98 126 unsigned char id;
iforce2d 0:972874f31c98 127 unsigned short len;
iforce2d 0:972874f31c98 128 unsigned long iTOW; // GPS time of week of the navigation epoch (ms)
iforce2d 0:972874f31c98 129
iforce2d 0:972874f31c98 130 unsigned short year; // Year (UTC)
iforce2d 0:972874f31c98 131 unsigned char month; // Month, range 1..12 (UTC)
iforce2d 0:972874f31c98 132 unsigned char day; // Day of month, range 1..31 (UTC)
iforce2d 0:972874f31c98 133 unsigned char hour; // Hour of day, range 0..23 (UTC)
iforce2d 0:972874f31c98 134 unsigned char minute; // Minute of hour, range 0..59 (UTC)
iforce2d 0:972874f31c98 135 unsigned char second; // Seconds of minute, range 0..60 (UTC)
iforce2d 0:972874f31c98 136 char valid; // Validity Flags (see graphic below)
iforce2d 0:972874f31c98 137 unsigned long tAcc; // Time accuracy estimate (UTC) (ns)
iforce2d 0:972874f31c98 138 long nano; // Fraction of second, range -1e9 .. 1e9 (UTC) (ns)
iforce2d 0:972874f31c98 139 unsigned char fixType; // GNSSfix Type, range 0..5
iforce2d 0:972874f31c98 140 char flags; // Fix Status Flags
iforce2d 0:972874f31c98 141 unsigned char reserved1; // reserved
iforce2d 0:972874f31c98 142 unsigned char numSV; // Number of satellites used in Nav Solution
iforce2d 0:972874f31c98 143
iforce2d 0:972874f31c98 144 long lon; // Longitude (deg)
iforce2d 0:972874f31c98 145 long lat; // Latitude (deg)
iforce2d 0:972874f31c98 146 long height; // Height above Ellipsoid (mm)
iforce2d 0:972874f31c98 147 long hMSL; // Height above mean sea level (mm)
iforce2d 0:972874f31c98 148 unsigned long hAcc; // Horizontal Accuracy Estimate (mm)
iforce2d 0:972874f31c98 149 unsigned long vAcc; // Vertical Accuracy Estimate (mm)
iforce2d 0:972874f31c98 150
iforce2d 0:972874f31c98 151 long velN; // NED north velocity (mm/s)
iforce2d 0:972874f31c98 152 long velE; // NED east velocity (mm/s)
iforce2d 0:972874f31c98 153 long velD; // NED down velocity (mm/s)
iforce2d 0:972874f31c98 154 long gSpeed; // Ground Speed (2-D) (mm/s)
iforce2d 0:972874f31c98 155 long heading; // Heading of motion 2-D (deg)
iforce2d 0:972874f31c98 156 unsigned long sAcc; // Speed Accuracy Estimate
iforce2d 0:972874f31c98 157 unsigned long headingAcc; // Heading Accuracy Estimate
iforce2d 0:972874f31c98 158 unsigned short pDOP; // Position dilution of precision
iforce2d 0:972874f31c98 159 short reserved2; // Reserved
iforce2d 0:972874f31c98 160 unsigned long reserved3; // Reserved
iforce2d 0:972874f31c98 161
iforce2d 0:972874f31c98 162 // Most receivers prior to the M8 series have an older firmware with a
iforce2d 0:972874f31c98 163 // smaller size NAV_PVT message. For Neo-7M you should comment this out.
iforce2d 0:972874f31c98 164 uint8_t dummy[8];
iforce2d 0:972874f31c98 165 };
iforce2d 0:972874f31c98 166
iforce2d 0:972874f31c98 167 NAV_PVT pvt;
iforce2d 0:972874f31c98 168
iforce2d 0:972874f31c98 169 void Rx_interrupt()
iforce2d 0:972874f31c98 170 {
iforce2d 0:972874f31c98 171 while ((gps.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
iforce2d 0:972874f31c98 172 rx_buffer[rx_in] = gps.getc();
iforce2d 0:972874f31c98 173 rx_in = (rx_in + 1) % buffer_size;
iforce2d 0:972874f31c98 174 }
iforce2d 0:972874f31c98 175 }
iforce2d 0:972874f31c98 176
iforce2d 0:972874f31c98 177 bool gpsAvailable()
iforce2d 0:972874f31c98 178 {
iforce2d 0:972874f31c98 179 return rx_in != rx_out;
iforce2d 0:972874f31c98 180 }
iforce2d 0:972874f31c98 181
iforce2d 0:972874f31c98 182 char gpsRead()
iforce2d 0:972874f31c98 183 {
iforce2d 0:972874f31c98 184 __disable_irq();
iforce2d 0:972874f31c98 185 char c = rx_buffer[rx_out];
iforce2d 0:972874f31c98 186 rx_out = (rx_out + 1) % buffer_size;
iforce2d 0:972874f31c98 187 __enable_irq();
iforce2d 0:972874f31c98 188 return c;
iforce2d 0:972874f31c98 189 }
iforce2d 0:972874f31c98 190
iforce2d 0:972874f31c98 191 void calcChecksum(unsigned char* CK)
iforce2d 0:972874f31c98 192 {
iforce2d 0:972874f31c98 193 memset(CK, 0, 2);
iforce2d 0:972874f31c98 194 for (int i = 0; i < (int)sizeof(NAV_PVT); i++) {
iforce2d 0:972874f31c98 195 CK[0] += ((unsigned char*)(&pvt))[i];
iforce2d 0:972874f31c98 196 CK[1] += CK[0];
iforce2d 0:972874f31c98 197 }
iforce2d 0:972874f31c98 198 }
iforce2d 0:972874f31c98 199
iforce2d 0:972874f31c98 200 bool processGPS()
iforce2d 0:972874f31c98 201 {
iforce2d 0:972874f31c98 202 static int fpos = 0;
iforce2d 0:972874f31c98 203 static unsigned char checksum[2];
iforce2d 0:972874f31c98 204 const int payloadSize = sizeof(NAV_PVT);
iforce2d 0:972874f31c98 205
iforce2d 0:972874f31c98 206 while ( gpsAvailable() ) {
iforce2d 0:972874f31c98 207 char c0 = gpsRead();
iforce2d 0:972874f31c98 208 unsigned char c = *(unsigned char*)(&c0);
iforce2d 0:972874f31c98 209 if ( fpos < 2 ) {
iforce2d 0:972874f31c98 210 if ( c == UBX_HEADER[fpos] )
iforce2d 0:972874f31c98 211 fpos++;
iforce2d 0:972874f31c98 212 else
iforce2d 0:972874f31c98 213 fpos = 0;
iforce2d 0:972874f31c98 214 } else {
iforce2d 0:972874f31c98 215 if ( (fpos-2) < payloadSize )
iforce2d 0:972874f31c98 216 ((unsigned char*)(&pvt))[fpos-2] = c;
iforce2d 0:972874f31c98 217
iforce2d 0:972874f31c98 218 fpos++;
iforce2d 0:972874f31c98 219
iforce2d 0:972874f31c98 220 if ( fpos == (payloadSize+2) ) {
iforce2d 0:972874f31c98 221 calcChecksum(checksum);
iforce2d 0:972874f31c98 222 } else if ( fpos == (payloadSize+3) ) {
iforce2d 0:972874f31c98 223 if ( c != checksum[0] )
iforce2d 0:972874f31c98 224 fpos = 0;
iforce2d 0:972874f31c98 225 } else if ( fpos == (payloadSize+4) ) {
iforce2d 0:972874f31c98 226 fpos = 0;
iforce2d 0:972874f31c98 227 if ( c == checksum[1] ) {
iforce2d 0:972874f31c98 228 myled = 1 - myled;
iforce2d 0:972874f31c98 229 return true;
iforce2d 0:972874f31c98 230 }
iforce2d 0:972874f31c98 231 } else if ( fpos > (payloadSize+4) ) {
iforce2d 0:972874f31c98 232 fpos = 0;
iforce2d 0:972874f31c98 233 }
iforce2d 0:972874f31c98 234 }
iforce2d 0:972874f31c98 235 }
iforce2d 0:972874f31c98 236 return false;
iforce2d 0:972874f31c98 237 }
iforce2d 0:972874f31c98 238
iforce2d 0:972874f31c98 239 u8g_t u8g;
iforce2d 0:972874f31c98 240
iforce2d 0:972874f31c98 241 double distance = 0;
iforce2d 0:972874f31c98 242 int numSV = 0;
iforce2d 0:972874f31c98 243 float hAcc = 0;
iforce2d 0:972874f31c98 244 uint64_t lastScreenUpdate = 0;
iforce2d 0:972874f31c98 245 char spinnerBuf[16];
iforce2d 0:972874f31c98 246 char satsBuf[32];
iforce2d 0:972874f31c98 247 char haccBuf[32];
iforce2d 0:972874f31c98 248 char distBuf[32];
iforce2d 0:972874f31c98 249
iforce2d 0:972874f31c98 250 char* spinnerChars = "/-\\|";
iforce2d 0:972874f31c98 251 uint8_t screenRefreshSpinnerPos = 0;
iforce2d 0:972874f31c98 252 uint8_t gpsUpdateSpinnerPos = 0;
iforce2d 0:972874f31c98 253
iforce2d 0:972874f31c98 254 void draw(void)
iforce2d 0:972874f31c98 255 {
iforce2d 0:972874f31c98 256 u8g_SetFont(&u8g, u8g_font_9x15B);
iforce2d 0:972874f31c98 257 u8g_DrawStr(&u8g, 2, 12, spinnerBuf);
iforce2d 0:972874f31c98 258
iforce2d 0:972874f31c98 259 u8g_SetFont(&u8g, u8g_font_helvB08);
iforce2d 0:972874f31c98 260 u8g_DrawStr(&u8g, 42, 8, satsBuf);
iforce2d 0:972874f31c98 261 u8g_DrawStr(&u8g, 42, 21, haccBuf);
iforce2d 0:972874f31c98 262
iforce2d 0:972874f31c98 263 u8g_SetFont(&u8g, u8g_font_fub30);
iforce2d 0:972874f31c98 264 u8g_DrawStr(&u8g, 2, 62, distBuf);
iforce2d 0:972874f31c98 265 }
iforce2d 0:972874f31c98 266
iforce2d 0:972874f31c98 267 void updateScreen()
iforce2d 0:972874f31c98 268 {
iforce2d 0:972874f31c98 269 sprintf(spinnerBuf, "%c %c", spinnerChars[screenRefreshSpinnerPos], spinnerChars[gpsUpdateSpinnerPos]);
iforce2d 0:972874f31c98 270 sprintf(satsBuf, "Sats: %d", numSV);
iforce2d 0:972874f31c98 271 sprintf(haccBuf, "hAcc: %d", (int)hAcc);
iforce2d 0:972874f31c98 272 sprintf(distBuf, "%.1lf", distance);
iforce2d 0:972874f31c98 273
iforce2d 0:972874f31c98 274 u8g_FirstPage(&u8g);
iforce2d 0:972874f31c98 275 do {
iforce2d 0:972874f31c98 276 draw();
iforce2d 0:972874f31c98 277 } while ( u8g_NextPage(&u8g) );
iforce2d 0:972874f31c98 278 u8g_Delay(10);
iforce2d 0:972874f31c98 279 }
iforce2d 0:972874f31c98 280
iforce2d 0:972874f31c98 281 void checkScreenUpdate()
iforce2d 0:972874f31c98 282 {
iforce2d 0:972874f31c98 283 uint64_t now = clock_ms();
iforce2d 0:972874f31c98 284 if ( now - lastScreenUpdate > 100 ) {
iforce2d 0:972874f31c98 285 updateScreen();
iforce2d 0:972874f31c98 286 lastScreenUpdate = now;
iforce2d 0:972874f31c98 287 screenRefreshSpinnerPos = (screenRefreshSpinnerPos + 1) % 4;
iforce2d 0:972874f31c98 288 }
iforce2d 0:972874f31c98 289 }
iforce2d 0:972874f31c98 290
iforce2d 0:972874f31c98 291 int main()
iforce2d 0:972874f31c98 292 {
iforce2d 0:972874f31c98 293 gps.attach(&Rx_interrupt, Serial::RxIrq);
iforce2d 0:972874f31c98 294 u8g_InitComFn(&u8g, &u8g_dev_ssd1306_128x64_i2c, u8g_com_hw_i2c_fn);
iforce2d 0:972874f31c98 295
iforce2d 0:972874f31c98 296 while ( ! processGPS() )
iforce2d 0:972874f31c98 297 checkScreenUpdate();
iforce2d 0:972874f31c98 298
iforce2d 0:972874f31c98 299 originLocation.lat = pvt.lat * 0.0000001;
iforce2d 0:972874f31c98 300 originLocation.lon = pvt.lon * 0.0000001;
iforce2d 0:972874f31c98 301
iforce2d 0:972874f31c98 302 while (true) {
iforce2d 0:972874f31c98 303
iforce2d 0:972874f31c98 304 if ( processGPS() ) {
iforce2d 0:972874f31c98 305 currentLocation.lat = pvt.lat * 0.0000001;
iforce2d 0:972874f31c98 306 currentLocation.lon = pvt.lon * 0.0000001;
iforce2d 0:972874f31c98 307 numSV = pvt.numSV;
iforce2d 0:972874f31c98 308 hAcc = pvt.hAcc;
iforce2d 0:972874f31c98 309 distance = geoDistance( currentLocation, originLocation ) * 100;
iforce2d 0:972874f31c98 310 gpsUpdateSpinnerPos = (gpsUpdateSpinnerPos + 1) % 4;
iforce2d 0:972874f31c98 311 }
iforce2d 0:972874f31c98 312
iforce2d 0:972874f31c98 313 checkScreenUpdate();
iforce2d 0:972874f31c98 314 }
iforce2d 0:972874f31c98 315 }
iforce2d 0:972874f31c98 316