Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

main.cpp@1:aef1562834b4, 2014-02-16 (annotated)

Committer:
stearnsc
Date:
Sun Feb 16 22:12:47 2014 +0000
Revision:
1:aef1562834b4
Parent:
0:9c001c4e7bf4 
cs: Enable DGPS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stearnsc 0:9c001c4e7bf4 1 #include "mbed.h"
stearnsc 0:9c001c4e7bf4 2 #include <string>
stearnsc 0:9c001c4e7bf4 3 #include <sstream>
stearnsc 0:9c001c4e7bf4 4
stearnsc 0:9c001c4e7bf4 5 Serial pc(USBTX,USBRX);
stearnsc 0:9c001c4e7bf4 6 Serial xbee(p9,p10);//tx, rx
stearnsc 0:9c001c4e7bf4 7 Serial gps(p28,p27);
stearnsc 0:9c001c4e7bf4 8
stearnsc 0:9c001c4e7bf4 9 typedef struct {
stearnsc 1:aef1562834b4 10 int latitude; //in 0.0001 minutes
stearnsc 1:aef1562834b4 11 int longitude; //in 0.0001 minutes
stearnsc 1:aef1562834b4 12 int altitude; //in 0.1 m
stearnsc 0:9c001c4e7bf4 13 int time; //in milliseconds
stearnsc 0:9c001c4e7bf4 14 } GpsData;
stearnsc 0:9c001c4e7bf4 15
stearnsc 0:9c001c4e7bf4 16 GpsData gpsData;
stearnsc 0:9c001c4e7bf4 17 GpsData otherGps;
stearnsc 0:9c001c4e7bf4 18
stearnsc 0:9c001c4e7bf4 19 void readSerial(Serial &s, char str[], int size){
stearnsc 0:9c001c4e7bf4 20 for (int i = 0; i < size; i++){
stearnsc 0:9c001c4e7bf4 21 str[i] = s.getc();
stearnsc 0:9c001c4e7bf4 22 }
stearnsc 0:9c001c4e7bf4 23 }
stearnsc 0:9c001c4e7bf4 24
stearnsc 0:9c001c4e7bf4 25 //sends: "$<command>*<checksum>\r\l"
stearnsc 0:9c001c4e7bf4 26 void sendGpsCommand(string command){
stearnsc 0:9c001c4e7bf4 27 uint8_t checksum = 0;
stearnsc 0:9c001c4e7bf4 28 pc.printf("Sending command to gps: ");
stearnsc 0:9c001c4e7bf4 29 gps.putc('$');
stearnsc 0:9c001c4e7bf4 30 pc.putc('$');
stearnsc 0:9c001c4e7bf4 31 char c;
stearnsc 0:9c001c4e7bf4 32 for (int i = 0; i < command.length(); i++){
stearnsc 0:9c001c4e7bf4 33 c = command[i];
stearnsc 0:9c001c4e7bf4 34 checksum ^= c;
stearnsc 0:9c001c4e7bf4 35 gps.putc(c);
stearnsc 0:9c001c4e7bf4 36 pc.putc(c);
stearnsc 0:9c001c4e7bf4 37 }
stearnsc 0:9c001c4e7bf4 38 gps.putc('*');
stearnsc 0:9c001c4e7bf4 39 pc.putc('*');
stearnsc 0:9c001c4e7bf4 40 string checkSumString;
stearnsc 0:9c001c4e7bf4 41 while (checksum > 0){
stearnsc 0:9c001c4e7bf4 42 uint8_t checksumChar = checksum & 0x0F;
stearnsc 0:9c001c4e7bf4 43 if (checksumChar >= 10){
stearnsc 0:9c001c4e7bf4 44 checksumChar -= 10;
stearnsc 0:9c001c4e7bf4 45 checksumChar += 'A';
stearnsc 0:9c001c4e7bf4 46 } else {
stearnsc 0:9c001c4e7bf4 47 checksumChar += '0';
stearnsc 0:9c001c4e7bf4 48 }
stearnsc 0:9c001c4e7bf4 49 checkSumString.push_back((char) checksumChar);
stearnsc 0:9c001c4e7bf4 50 checksum = checksum >> 4;
stearnsc 0:9c001c4e7bf4 51 }
stearnsc 0:9c001c4e7bf4 52
stearnsc 0:9c001c4e7bf4 53 for (int i = checkSumString.length() - 1; i >= 0; i--){
stearnsc 0:9c001c4e7bf4 54 gps.putc(checkSumString[i]);
stearnsc 0:9c001c4e7bf4 55 pc.putc(checkSumString[i]);
stearnsc 0:9c001c4e7bf4 56 }
stearnsc 0:9c001c4e7bf4 57 gps.putc('\r');
stearnsc 0:9c001c4e7bf4 58 pc.putc('\r');
stearnsc 0:9c001c4e7bf4 59 gps.putc('\n');
stearnsc 0:9c001c4e7bf4 60 pc.putc('\n');
stearnsc 0:9c001c4e7bf4 61 }
stearnsc 0:9c001c4e7bf4 62
stearnsc 0:9c001c4e7bf4 63 long parseDec(string s){
stearnsc 0:9c001c4e7bf4 64 int mult = 1;
stearnsc 0:9c001c4e7bf4 65 int result = 0;
stearnsc 0:9c001c4e7bf4 66 for (int i = s.length() - 1; i >=0; i--){
stearnsc 0:9c001c4e7bf4 67 if (s[i] != '.'){
stearnsc 0:9c001c4e7bf4 68 result += (s[i] - '0') * mult;
stearnsc 0:9c001c4e7bf4 69 mult *= 10;
stearnsc 0:9c001c4e7bf4 70 }
stearnsc 0:9c001c4e7bf4 71 }
stearnsc 0:9c001c4e7bf4 72 return result;
stearnsc 0:9c001c4e7bf4 73 }
stearnsc 0:9c001c4e7bf4 74
stearnsc 0:9c001c4e7bf4 75 //cs: little endian parsing
stearnsc 0:9c001c4e7bf4 76 int nextInt(char *data, int i){
stearnsc 0:9c001c4e7bf4 77 i |= data[i];
stearnsc 0:9c001c4e7bf4 78 i |= (data[i + 1] << 8);
stearnsc 0:9c001c4e7bf4 79 i |= (data[i + 2] << 16);
stearnsc 0:9c001c4e7bf4 80 i |= (data[i + 3] << 24);
stearnsc 0:9c001c4e7bf4 81 return i;
stearnsc 0:9c001c4e7bf4 82 }
stearnsc 0:9c001c4e7bf4 83
stearnsc 0:9c001c4e7bf4 84 void handleXbeeGps(){
stearnsc 0:9c001c4e7bf4 85 static bool reading = false;
stearnsc 0:9c001c4e7bf4 86 static char packet[16];
stearnsc 0:9c001c4e7bf4 87 static int i = 0;
stearnsc 0:9c001c4e7bf4 88
stearnsc 0:9c001c4e7bf4 89 char c = xbee.getc();
stearnsc 0:9c001c4e7bf4 90 if (reading){
stearnsc 0:9c001c4e7bf4 91 packet[i] = c;
stearnsc 0:9c001c4e7bf4 92 i++;
stearnsc 0:9c001c4e7bf4 93 if (i == 16){
stearnsc 0:9c001c4e7bf4 94 i = 0;
stearnsc 0:9c001c4e7bf4 95 otherGps.latitude = nextInt(packet, 0);
stearnsc 0:9c001c4e7bf4 96 otherGps.longitude = nextInt(packet, 4);
stearnsc 0:9c001c4e7bf4 97 otherGps.altitude = nextInt(packet, 8);
stearnsc 0:9c001c4e7bf4 98 otherGps.time = nextInt(packet, 12);
stearnsc 0:9c001c4e7bf4 99
stearnsc 0:9c001c4e7bf4 100 pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 0:9c001c4e7bf4 101 otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time
stearnsc 0:9c001c4e7bf4 102 );
stearnsc 0:9c001c4e7bf4 103 reading = false;
stearnsc 1:aef1562834b4 104 pc.printf("Lat: %d, Lon: %d, Alt: %d, Time: %d\r\n",
stearnsc 1:aef1562834b4 105 gpsData.latitude - otherGps.latitude,
stearnsc 1:aef1562834b4 106 gpsData.longitude - otherGps.longitude,
stearnsc 1:aef1562834b4 107 gpsData.altitude - otherGps.altitude,
stearnsc 1:aef1562834b4 108 gpsData.time - otherGps.time
stearnsc 1:aef1562834b4 109 );
stearnsc 0:9c001c4e7bf4 110 }
stearnsc 0:9c001c4e7bf4 111 } else if (c == 'X'){
stearnsc 0:9c001c4e7bf4 112 reading = true;
stearnsc 0:9c001c4e7bf4 113 }
stearnsc 0:9c001c4e7bf4 114 }
stearnsc 0:9c001c4e7bf4 115
stearnsc 0:9c001c4e7bf4 116 void handleGpsData(){
stearnsc 0:9c001c4e7bf4 117 static bool reading = false;
stearnsc 0:9c001c4e7bf4 118 static stringstream line;
stearnsc 0:9c001c4e7bf4 119
stearnsc 0:9c001c4e7bf4 120 char c = gps.getc();
stearnsc 0:9c001c4e7bf4 121
stearnsc 0:9c001c4e7bf4 122 if (reading){
stearnsc 0:9c001c4e7bf4 123 if (c == '*'){ //sentence buffer complete; we're ignoring the checksum
stearnsc 0:9c001c4e7bf4 124 string field;
stearnsc 0:9c001c4e7bf4 125 std::getline(line, field, ','); //GPGGA
stearnsc 0:9c001c4e7bf4 126 std::getline(line, field, ','); //time
stearnsc 0:9c001c4e7bf4 127 gpsData.time = parseDec(field);
stearnsc 0:9c001c4e7bf4 128 std::getline(line, field, ','); //latitude
stearnsc 0:9c001c4e7bf4 129 gpsData.latitude = parseDec(field);
stearnsc 0:9c001c4e7bf4 130 std::getline(line, field, ','); //N or S
stearnsc 0:9c001c4e7bf4 131 std::getline(line, field, ','); //longitude
stearnsc 0:9c001c4e7bf4 132 gpsData.longitude = parseDec(field);
stearnsc 0:9c001c4e7bf4 133 std::getline(line, field, ','); //E or W
stearnsc 0:9c001c4e7bf4 134 std::getline(line, field, ','); //skip
stearnsc 0:9c001c4e7bf4 135 std::getline(line, field, ','); //skip
stearnsc 0:9c001c4e7bf4 136 std::getline(line, field, ','); //skip
stearnsc 0:9c001c4e7bf4 137 std::getline(line, field, ','); //altitude
stearnsc 0:9c001c4e7bf4 138 gpsData.altitude = parseDec(field);
stearnsc 0:9c001c4e7bf4 139
stearnsc 0:9c001c4e7bf4 140 //update whatever needs updating when gps updates
stearnsc 1:aef1562834b4 141 // pc.printf("My GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 1:aef1562834b4 142 // gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time
stearnsc 1:aef1562834b4 143 // );
stearnsc 0:9c001c4e7bf4 144
stearnsc 0:9c001c4e7bf4 145 line.str(string(""));
stearnsc 0:9c001c4e7bf4 146 reading = false;
stearnsc 0:9c001c4e7bf4 147
stearnsc 0:9c001c4e7bf4 148 } else {
stearnsc 0:9c001c4e7bf4 149 line.put(c);
stearnsc 0:9c001c4e7bf4 150 }
stearnsc 0:9c001c4e7bf4 151
stearnsc 0:9c001c4e7bf4 152 } else if (c == '$') {
stearnsc 0:9c001c4e7bf4 153 reading = true;
stearnsc 0:9c001c4e7bf4 154 }
stearnsc 0:9c001c4e7bf4 155 }
stearnsc 0:9c001c4e7bf4 156
stearnsc 0:9c001c4e7bf4 157 int main(){
stearnsc 0:9c001c4e7bf4 158 gps.baud(57600);
stearnsc 0:9c001c4e7bf4 159 xbee.baud(9600);
stearnsc 0:9c001c4e7bf4 160 pc.baud(57600);
stearnsc 0:9c001c4e7bf4 161
stearnsc 0:9c001c4e7bf4 162 // sendGpsCommand("PMTK301,1");
stearnsc 0:9c001c4e7bf4 163 gps.attach(&handleGpsData, Serial::RxIrq);
stearnsc 1:aef1562834b4 164 xbee.attach(&handleXbeeGps, Serial::RxIrq);
stearnsc 0:9c001c4e7bf4 165 }