iforce2d Chris
Displays distance to start location on OLED screen.
Diff: main.cpp
- Revision:
- 0:972874f31c98
diff -r 000000000000 -r 972874f31c98 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Mar 07 12:49:14 2018 +0000
@@ -0,0 +1,316 @@
+/*
+Reads UBX NAV-PVT messages from a u-blox GPS module and displays the distance in
+centimeters between the current location and the first location received at power
+up. No configuration messages are sent to the GPS so it needs to be prepared to
+output NAV-PVT messages ONLY, and have that configuration saved so it will always
+do so when powered up.
+Screen is a SSD1306 running on I2C connection.
+*/
+#include "mbed.h"
+#include "Clock.h"
+#include "u8g.h"
+
+#define OLED_ADDRESS 0x78
+
+DigitalOut myled(PB_1);
+I2C i2c(PB_7, PB_6); // sda, scl
+Serial gps(PA_2, PA_3, 19200); // tx, rx
+
+// At 19200 baud the buffer gets overwritten too quickly. We need to make our
+// own software buffer and update it by interrupts so we don't miss anything.
+void Rx_interrupt();
+const int buffer_size = 1024;
+char rx_buffer[buffer_size+1];
+volatile int rx_in=0;
+volatile int rx_out=0;
+
+
+void u8g_Delay(uint16_t usec)
+{
+ wait_us((int)usec);
+}
+
+void u8g_MicroDelay()
+{
+ wait_us(1);
+}
+
+void u8g_10MicroDelay()
+{
+ wait_us(10);
+}
+
+uint8_t u8g_com_hw_i2c_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr)
+{
+ static uint8_t control = 0;
+
+ switch(msg) {
+ case U8G_COM_MSG_STOP:
+ break; //do nothing...
+
+ case U8G_COM_MSG_INIT:
+ u8g_MicroDelay();
+ break;
+
+ case U8G_COM_MSG_ADDRESS:
+ //the control byte switches the mode on the device and is set here
+ u8g_10MicroDelay();
+ if (arg_val == 0) {
+ control = 0;
+ } else {
+ control = 0x40;
+ }
+ break;
+
+ case U8G_COM_MSG_WRITE_BYTE: {
+ //simple: just write one byte
+ char buffer[2];
+ buffer[0] = control;
+ buffer[1] = arg_val;
+ i2c.write(OLED_ADDRESS, buffer, 2);
+ }
+ break;
+
+ case U8G_COM_MSG_WRITE_SEQ:
+ case U8G_COM_MSG_WRITE_SEQ_P: {
+ char buffer[1024];
+ char *ptr = (char*)arg_ptr;
+ buffer[0] = control;
+ for (int i = 1; i <= arg_val; i++) {
+ buffer[i] = *(ptr++);
+ }
+ i2c.write(OLED_ADDRESS, buffer, arg_val);
+ }
+ break;
+
+ }
+ return 1;
+}
+
+#ifndef DEGTORAD
+#define DEGTORAD 0.0174532925199432957f
+#define RADTODEG 57.295779513082320876f
+#endif
+
+struct geoloc {
+ double lat;
+ double lon;
+ geoloc(double x = 0, double y = 0, int32_t a = 0) {
+ lat = x;
+ lon = y;
+ }
+};
+
+// http://www.movable-type.co.uk/scripts/latlong.html
+float geoDistance(struct geoloc &a, struct geoloc &b) // returns meters
+{
+ const double R = 6371000; // km
+ double p1 = a.lat * DEGTORAD;
+ double p2 = b.lat * DEGTORAD;
+ double dp = (b.lat-a.lat) * DEGTORAD;
+ double dl = (b.lon-a.lon) * DEGTORAD;
+
+ double x = sin(dp/2) * sin(dp/2) + cos(p1) * cos(p2) * sin(dl/2) * sin(dl/2);
+ double y = 2 * atan2(sqrt(x), sqrt(1-x));
+
+ return R * y;
+}
+
+geoloc originLocation;
+geoloc currentLocation;
+
+const unsigned char UBX_HEADER[] = { 0xB5, 0x62 };
+
+struct NAV_PVT {
+ unsigned char cls;
+ unsigned char id;
+ unsigned short len;
+ unsigned long iTOW; // GPS time of week of the navigation epoch (ms)
+
+ unsigned short year; // Year (UTC)
+ unsigned char month; // Month, range 1..12 (UTC)
+ unsigned char day; // Day of month, range 1..31 (UTC)
+ unsigned char hour; // Hour of day, range 0..23 (UTC)
+ unsigned char minute; // Minute of hour, range 0..59 (UTC)
+ unsigned char second; // Seconds of minute, range 0..60 (UTC)
+ char valid; // Validity Flags (see graphic below)
+ unsigned long tAcc; // Time accuracy estimate (UTC) (ns)
+ long nano; // Fraction of second, range -1e9 .. 1e9 (UTC) (ns)
+ unsigned char fixType; // GNSSfix Type, range 0..5
+ char flags; // Fix Status Flags
+ unsigned char reserved1; // reserved
+ unsigned char numSV; // Number of satellites used in Nav Solution
+
+ long lon; // Longitude (deg)
+ long lat; // Latitude (deg)
+ long height; // Height above Ellipsoid (mm)
+ long hMSL; // Height above mean sea level (mm)
+ unsigned long hAcc; // Horizontal Accuracy Estimate (mm)
+ unsigned long vAcc; // Vertical Accuracy Estimate (mm)
+
+ long velN; // NED north velocity (mm/s)
+ long velE; // NED east velocity (mm/s)
+ long velD; // NED down velocity (mm/s)
+ long gSpeed; // Ground Speed (2-D) (mm/s)
+ long heading; // Heading of motion 2-D (deg)
+ unsigned long sAcc; // Speed Accuracy Estimate
+ unsigned long headingAcc; // Heading Accuracy Estimate
+ unsigned short pDOP; // Position dilution of precision
+ short reserved2; // Reserved
+ unsigned long reserved3; // Reserved
+
+ // Most receivers prior to the M8 series have an older firmware with a
+ // smaller size NAV_PVT message. For Neo-7M you should comment this out.
+ uint8_t dummy[8];
+};
+
+NAV_PVT pvt;
+
+void Rx_interrupt()
+{
+ while ((gps.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
+ rx_buffer[rx_in] = gps.getc();
+ rx_in = (rx_in + 1) % buffer_size;
+ }
+}
+
+bool gpsAvailable()
+{
+ return rx_in != rx_out;
+}
+
+char gpsRead()
+{
+ __disable_irq();
+ char c = rx_buffer[rx_out];
+ rx_out = (rx_out + 1) % buffer_size;
+ __enable_irq();
+ return c;
+}
+
+void calcChecksum(unsigned char* CK)
+{
+ memset(CK, 0, 2);
+ for (int i = 0; i < (int)sizeof(NAV_PVT); i++) {
+ CK[0] += ((unsigned char*)(&pvt))[i];
+ CK[1] += CK[0];
+ }
+}
+
+bool processGPS()
+{
+ static int fpos = 0;
+ static unsigned char checksum[2];
+ const int payloadSize = sizeof(NAV_PVT);
+
+ while ( gpsAvailable() ) {
+ char c0 = gpsRead();
+ unsigned char c = *(unsigned char*)(&c0);
+ if ( fpos < 2 ) {
+ if ( c == UBX_HEADER[fpos] )
+ fpos++;
+ else
+ fpos = 0;
+ } else {
+ if ( (fpos-2) < payloadSize )
+ ((unsigned char*)(&pvt))[fpos-2] = c;
+
+ fpos++;
+
+ if ( fpos == (payloadSize+2) ) {
+ calcChecksum(checksum);
+ } else if ( fpos == (payloadSize+3) ) {
+ if ( c != checksum[0] )
+ fpos = 0;
+ } else if ( fpos == (payloadSize+4) ) {
+ fpos = 0;
+ if ( c == checksum[1] ) {
+ myled = 1 - myled;
+ return true;
+ }
+ } else if ( fpos > (payloadSize+4) ) {
+ fpos = 0;
+ }
+ }
+ }
+ return false;
+}
+
+u8g_t u8g;
+
+double distance = 0;
+int numSV = 0;
+float hAcc = 0;
+uint64_t lastScreenUpdate = 0;
+char spinnerBuf[16];
+char satsBuf[32];
+char haccBuf[32];
+char distBuf[32];
+
+char* spinnerChars = "/-\\|";
+uint8_t screenRefreshSpinnerPos = 0;
+uint8_t gpsUpdateSpinnerPos = 0;
+
+void draw(void)
+{
+ u8g_SetFont(&u8g, u8g_font_9x15B);
+ u8g_DrawStr(&u8g, 2, 12, spinnerBuf);
+
+ u8g_SetFont(&u8g, u8g_font_helvB08);
+ u8g_DrawStr(&u8g, 42, 8, satsBuf);
+ u8g_DrawStr(&u8g, 42, 21, haccBuf);
+
+ u8g_SetFont(&u8g, u8g_font_fub30);
+ u8g_DrawStr(&u8g, 2, 62, distBuf);
+}
+
+void updateScreen()
+{
+ sprintf(spinnerBuf, "%c %c", spinnerChars[screenRefreshSpinnerPos], spinnerChars[gpsUpdateSpinnerPos]);
+ sprintf(satsBuf, "Sats: %d", numSV);
+ sprintf(haccBuf, "hAcc: %d", (int)hAcc);
+ sprintf(distBuf, "%.1lf", distance);
+
+ u8g_FirstPage(&u8g);
+ do {
+ draw();
+ } while ( u8g_NextPage(&u8g) );
+ u8g_Delay(10);
+}
+
+void checkScreenUpdate()
+{
+ uint64_t now = clock_ms();
+ if ( now - lastScreenUpdate > 100 ) {
+ updateScreen();
+ lastScreenUpdate = now;
+ screenRefreshSpinnerPos = (screenRefreshSpinnerPos + 1) % 4;
+ }
+}
+
+int main()
+{
+ gps.attach(&Rx_interrupt, Serial::RxIrq);
+ u8g_InitComFn(&u8g, &u8g_dev_ssd1306_128x64_i2c, u8g_com_hw_i2c_fn);
+
+ while ( ! processGPS() )
+ checkScreenUpdate();
+
+ originLocation.lat = pvt.lat * 0.0000001;
+ originLocation.lon = pvt.lon * 0.0000001;
+
+ while (true) {
+
+ if ( processGPS() ) {
+ currentLocation.lat = pvt.lat * 0.0000001;
+ currentLocation.lon = pvt.lon * 0.0000001;
+ numSV = pvt.numSV;
+ hAcc = pvt.hAcc;
+ distance = geoDistance( currentLocation, originLocation ) * 100;
+ gpsUpdateSpinnerPos = (gpsUpdateSpinnerPos + 1) % 4;
+ }
+
+ checkScreenUpdate();
+ }
+}
+