File content as of revision 6:ad588fd8948a:

#include "mbed.h"
Serial pc(USBTX, USBRX);

Serial gps(PB_6, PA_10);

int encodeUBX(unsigned char ubxClass, unsigned char ubxID, int payloadLength, unsigned char *ubxPayload);
int decodeUBX();

int main(){
    
    int nl = 0;
    unsigned char test[0] = {};
    char c;
    int rtn = -7;

    pc.printf("About to make UBX packet!\n\r");
    pc.printf("Sizeof: %i\n\r", sizeof(test));
    encodeUBX(0x0A, 0x09, sizeof(test), test);
    pc.printf("UBX packet made.\n\r");
    
   while(1) {
       if(gps.readable()) {
           nl ++;
           c = gps.getc();
           if (c == 0xB5) {
                pc.printf("Maybe a packet...\n\r");
                rtn = decodeUBX();
                if (rtn == 0) {
                    pc.printf("!!!Valid packet!");
                } else if (rtn == 1) {
                    pc.printf("!!!Checksum fail");
                } else if (rtn == -1) {
                    pc.printf("Not a packet.");
                }
            }
       pc.printf("%c", c);
        }
    }
}



int encodeUBX(unsigned char ubxClass, unsigned char ubxID, int payloadLength, unsigned char ubxPayload[]) {
    unsigned char ubxMessage[2];
    int i, CK_A, CK_B;
    int packetLength = 6 + payloadLength + 2;

    unsigned char packet[packetLength];
    packet[0] = 0xB5;
    packet[1] = 0x62;
    packet[2] = ubxClass;
    packet[3] = ubxID;
    packet[4] = payloadLength >> 0;
    packet[5] = payloadLength >> 8;
    
    for (i = 6; i < (6 + payloadLength); i++) {
        packet[i] = ubxPayload[i-6];
    }
    
    CK_A = 0;
    CK_B = 0;
    for (i = 2; i < (packetLength - 2); i++) {
        CK_A = CK_A + packet[i];
        CK_B = CK_B + CK_A;
    }
    
    packet[packetLength - 2] = CK_A;
    packet[packetLength - 1] = CK_B;
    
    pc.printf("Packet: ");
    for (i = 0; i < packetLength; i++) {
       pc.printf("%02x ", packet[i]);
    }
    
    
    pc.printf("\n\r");
       
    for (i = 0; i < packetLength; i++) {
      gps.putc(packet[i]);
    }
       
       
    return 0;
}

int decodeUBX() {
    int i = 0;
    if (gps.getc() != 0x62) {
        pc.printf("NOUBX");
        //Then it's not a UBX packet. =(
        return -1;
    } 
    unsigned char ubxClass, ubxID, CK_A, CK_B, CK_Acalc, CK_Bcalc, lenMSB, lenLSB;
    int packetLength;
    
    pc.printf("Reading Class, ID and packet legnth\n\r");
    ubxClass = gps.getc();
    ubxID = gps.getc();
    lenLSB = gps.getc();
    lenMSB = gps.getc();
    packetLength = lenLSB + (lenMSB << 8);
    pc.printf("Class: %02x, ID: %02x \n\r", ubxClass, ubxID);
    pc.printf("lenU: %02x, lenL: %02x, packetLength: %i\n\r", lenMSB, lenLSB, packetLength); 
    
    unsigned char ubxPayload[packetLength];
    
    pc.printf("Reading packet of length %i\n\r", packetLength);
    for (i = 0; i < packetLength; i++) {
        ubxPayload[i] = gps.getc();    
    }
    pc.printf("Done reading packet, getting checksum.\n\r");
    CK_A = gps.getc();
    CK_B = gps.getc();   
    
    pc.printf("Calculating checksum.\n\r");
    CK_Acalc = 0;
    CK_Bcalc = 0;
    
    for (i = 2; i < (packetLength - 2); i++) {
        CK_Acalc = CK_Acalc + ubxPayload[i];
        CK_Bcalc = CK_Bcalc + CK_Acalc;
    }
    pc.printf("Done calculating checksum.\n\r");
    if ((CK_A == CK_Acalc) && (CK_B == CK_Bcalc)) {
       return 0;
    } else {
       return 1;
    }
}