File content as of revision 44:0d72a8a1288a:

#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;
    _callback = NULL;
}

int Ublox6::parse(int 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++;
            }
            break;
        case 1:    // wait for sync 2 (0x62)
            if (cc == SYNC2) {
                state++;
            } else {
                state = 0;
            }
            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;            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) {
                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;
                        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;
                        _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;
                        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;                                
                        break;
                    default:
                        break;
                    }
                    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;
        _ready = 0;
        status = 1;

        // clear tmp
        tmp.lat = 0;
        tmp.lon = 0;
        tmp.speed = 0;
        tmp.course = 0;
        tmp.hdop = 0;
        tmp.svcount = 0;
        if (_callback)
            _callback();
    }

    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;
}


void Ublox6::subscribe(Callback<void()> cb) {
    _callback = cb;

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