SurreyEARS / Mbed 2 deprecated EARS-FlightCTRL

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed USBDevice

main.cpp@6:ad588fd8948a, 2015-08-01 (annotated)

Committer:
wikrok
Date:
Sat Aug 01 21:18:31 2015 +0000
Branch:
dev-uBlox
Revision:
6:ad588fd8948a
Parent:
3:2e087d032875 
It's not working so I'm switching to the barometer.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
captaingoujon 0:27c53e093a59 1 #include "mbed.h"
wikrok 6:ad588fd8948a 2 Serial pc(USBTX, USBRX);
wikrok 6:ad588fd8948a 3
wikrok 6:ad588fd8948a 4 Serial gps(PB_6, PA_10);
wikrok 6:ad588fd8948a 5
wikrok 6:ad588fd8948a 6 int encodeUBX(unsigned char ubxClass, unsigned char ubxID, int payloadLength, unsigned char *ubxPayload);
wikrok 6:ad588fd8948a 7 int decodeUBX();
captaingoujon 0:27c53e093a59 8
captaingoujon 0:27c53e093a59 9 int main(){
wikrok 6:ad588fd8948a 10
wikrok 6:ad588fd8948a 11 int nl = 0;
wikrok 6:ad588fd8948a 12 unsigned char test[0] = {};
wikrok 6:ad588fd8948a 13 char c;
wikrok 6:ad588fd8948a 14 int rtn = -7;
wikrok 6:ad588fd8948a 15
wikrok 6:ad588fd8948a 16 pc.printf("About to make UBX packet!\n\r");
wikrok 6:ad588fd8948a 17 pc.printf("Sizeof: %i\n\r", sizeof(test));
wikrok 6:ad588fd8948a 18 encodeUBX(0x0A, 0x09, sizeof(test), test);
wikrok 6:ad588fd8948a 19 pc.printf("UBX packet made.\n\r");
wikrok 6:ad588fd8948a 20
wikrok 6:ad588fd8948a 21 while(1) {
wikrok 6:ad588fd8948a 22 if(gps.readable()) {
wikrok 6:ad588fd8948a 23 nl ++;
wikrok 6:ad588fd8948a 24 c = gps.getc();
wikrok 6:ad588fd8948a 25 if (c == 0xB5) {
wikrok 6:ad588fd8948a 26 pc.printf("Maybe a packet...\n\r");
wikrok 6:ad588fd8948a 27 rtn = decodeUBX();
wikrok 6:ad588fd8948a 28 if (rtn == 0) {
wikrok 6:ad588fd8948a 29 pc.printf("!!!Valid packet!");
wikrok 6:ad588fd8948a 30 } else if (rtn == 1) {
wikrok 6:ad588fd8948a 31 pc.printf("!!!Checksum fail");
wikrok 6:ad588fd8948a 32 } else if (rtn == -1) {
wikrok 6:ad588fd8948a 33 pc.printf("Not a packet.");
wikrok 6:ad588fd8948a 34 }
wikrok 6:ad588fd8948a 35 }
wikrok 6:ad588fd8948a 36 pc.printf("%c", c);
wikrok 6:ad588fd8948a 37 }
wikrok 6:ad588fd8948a 38 }
wikrok 6:ad588fd8948a 39 }
wikrok 6:ad588fd8948a 40
wikrok 6:ad588fd8948a 41
wikrok 6:ad588fd8948a 42
wikrok 6:ad588fd8948a 43 int encodeUBX(unsigned char ubxClass, unsigned char ubxID, int payloadLength, unsigned char ubxPayload[]) {
wikrok 6:ad588fd8948a 44 unsigned char ubxMessage[2];
wikrok 6:ad588fd8948a 45 int i, CK_A, CK_B;
wikrok 6:ad588fd8948a 46 int packetLength = 6 + payloadLength + 2;
wikrok 6:ad588fd8948a 47
wikrok 6:ad588fd8948a 48 unsigned char packet[packetLength];
wikrok 6:ad588fd8948a 49 packet[0] = 0xB5;
wikrok 6:ad588fd8948a 50 packet[1] = 0x62;
wikrok 6:ad588fd8948a 51 packet[2] = ubxClass;
wikrok 6:ad588fd8948a 52 packet[3] = ubxID;
wikrok 6:ad588fd8948a 53 packet[4] = payloadLength >> 0;
wikrok 6:ad588fd8948a 54 packet[5] = payloadLength >> 8;
wikrok 6:ad588fd8948a 55
wikrok 6:ad588fd8948a 56 for (i = 6; i < (6 + payloadLength); i++) {
wikrok 6:ad588fd8948a 57 packet[i] = ubxPayload[i-6];
wikrok 6:ad588fd8948a 58 }
wikrok 6:ad588fd8948a 59
wikrok 6:ad588fd8948a 60 CK_A = 0;
wikrok 6:ad588fd8948a 61 CK_B = 0;
wikrok 6:ad588fd8948a 62 for (i = 2; i < (packetLength - 2); i++) {
wikrok 6:ad588fd8948a 63 CK_A = CK_A + packet[i];
wikrok 6:ad588fd8948a 64 CK_B = CK_B + CK_A;
wikrok 6:ad588fd8948a 65 }
wikrok 6:ad588fd8948a 66
wikrok 6:ad588fd8948a 67 packet[packetLength - 2] = CK_A;
wikrok 6:ad588fd8948a 68 packet[packetLength - 1] = CK_B;
wikrok 6:ad588fd8948a 69
wikrok 6:ad588fd8948a 70 pc.printf("Packet: ");
wikrok 6:ad588fd8948a 71 for (i = 0; i < packetLength; i++) {
wikrok 6:ad588fd8948a 72 pc.printf("%02x ", packet[i]);
wikrok 6:ad588fd8948a 73 }
wikrok 6:ad588fd8948a 74
wikrok 6:ad588fd8948a 75
wikrok 6:ad588fd8948a 76 pc.printf("\n\r");
wikrok 6:ad588fd8948a 77
wikrok 6:ad588fd8948a 78 for (i = 0; i < packetLength; i++) {
wikrok 6:ad588fd8948a 79 gps.putc(packet[i]);
wikrok 6:ad588fd8948a 80 }
wikrok 6:ad588fd8948a 81
wikrok 6:ad588fd8948a 82
wikrok 6:ad588fd8948a 83 return 0;
wikrok 6:ad588fd8948a 84 }
wikrok 6:ad588fd8948a 85
wikrok 6:ad588fd8948a 86 int decodeUBX() {
wikrok 6:ad588fd8948a 87 int i = 0;
wikrok 6:ad588fd8948a 88 if (gps.getc() != 0x62) {
wikrok 6:ad588fd8948a 89 pc.printf("NOUBX");
wikrok 6:ad588fd8948a 90 //Then it's not a UBX packet. =(
wikrok 6:ad588fd8948a 91 return -1;
wikrok 6:ad588fd8948a 92 }
wikrok 6:ad588fd8948a 93 unsigned char ubxClass, ubxID, CK_A, CK_B, CK_Acalc, CK_Bcalc, lenMSB, lenLSB;
wikrok 6:ad588fd8948a 94 int packetLength;
wikrok 6:ad588fd8948a 95
wikrok 6:ad588fd8948a 96 pc.printf("Reading Class, ID and packet legnth\n\r");
wikrok 6:ad588fd8948a 97 ubxClass = gps.getc();
wikrok 6:ad588fd8948a 98 ubxID = gps.getc();
wikrok 6:ad588fd8948a 99 lenLSB = gps.getc();
wikrok 6:ad588fd8948a 100 lenMSB = gps.getc();
wikrok 6:ad588fd8948a 101 packetLength = lenLSB + (lenMSB << 8);
wikrok 6:ad588fd8948a 102 pc.printf("Class: %02x, ID: %02x \n\r", ubxClass, ubxID);
wikrok 6:ad588fd8948a 103 pc.printf("lenU: %02x, lenL: %02x, packetLength: %i\n\r", lenMSB, lenLSB, packetLength);
wikrok 6:ad588fd8948a 104
wikrok 6:ad588fd8948a 105 unsigned char ubxPayload[packetLength];
wikrok 6:ad588fd8948a 106
wikrok 6:ad588fd8948a 107 pc.printf("Reading packet of length %i\n\r", packetLength);
wikrok 6:ad588fd8948a 108 for (i = 0; i < packetLength; i++) {
wikrok 6:ad588fd8948a 109 ubxPayload[i] = gps.getc();
wikrok 6:ad588fd8948a 110 }
wikrok 6:ad588fd8948a 111 pc.printf("Done reading packet, getting checksum.\n\r");
wikrok 6:ad588fd8948a 112 CK_A = gps.getc();
wikrok 6:ad588fd8948a 113 CK_B = gps.getc();
wikrok 6:ad588fd8948a 114
wikrok 6:ad588fd8948a 115 pc.printf("Calculating checksum.\n\r");
wikrok 6:ad588fd8948a 116 CK_Acalc = 0;
wikrok 6:ad588fd8948a 117 CK_Bcalc = 0;
wikrok 6:ad588fd8948a 118
wikrok 6:ad588fd8948a 119 for (i = 2; i < (packetLength - 2); i++) {
wikrok 6:ad588fd8948a 120 CK_Acalc = CK_Acalc + ubxPayload[i];
wikrok 6:ad588fd8948a 121 CK_Bcalc = CK_Bcalc + CK_Acalc;
wikrok 6:ad588fd8948a 122 }
wikrok 6:ad588fd8948a 123 pc.printf("Done calculating checksum.\n\r");
wikrok 6:ad588fd8948a 124 if ((CK_A == CK_Acalc) && (CK_B == CK_Bcalc)) {
wikrok 6:ad588fd8948a 125 return 0;
wikrok 6:ad588fd8948a 126 } else {
wikrok 6:ad588fd8948a 127 return 1;
wikrok 6:ad588fd8948a 128 }
captaingoujon 0:27c53e093a59 129 }