File content as of revision 0:972874f31c98:

/*
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();
    }
}