Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

main.cpp@5:7bc8bec23b03, 2014-04-01 (annotated)

Committer:
stearnsc
Date:
Tue Apr 01 15:53:17 2014 +0000
Revision:
5:7bc8bec23b03
Parent:
4:c6daeab4a13b 
cs: not sure what I'm committing here..

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 0:9c001c4e7bf4 10 int latitude; //in .0001 minutes
stearnsc 0:9c001c4e7bf4 11 int longitude; //in .0001 minutes
stearnsc 4:c6daeab4a13b 12 } GpsCoordinates;
stearnsc 4:c6daeab4a13b 13
stearnsc 4:c6daeab4a13b 14 typedef struct {
stearnsc 4:c6daeab4a13b 15 int latitude; //in .0001 minutes
stearnsc 4:c6daeab4a13b 16 int longitude; //in .0001 minutes
stearnsc 0:9c001c4e7bf4 17 int altitude; //in decimeters
stearnsc 0:9c001c4e7bf4 18 int time; //in milliseconds
stearnsc 0:9c001c4e7bf4 19 } GpsData;
stearnsc 0:9c001c4e7bf4 20
stearnsc 0:9c001c4e7bf4 21 GpsData gpsData;
stearnsc 0:9c001c4e7bf4 22 GpsData otherGps;
stearnsc 0:9c001c4e7bf4 23
stearnsc 4:c6daeab4a13b 24 GpsCoordinates route[256];
stearnsc 4:c6daeab4a13b 25 uint8_t routeLength;
stearnsc 4:c6daeab4a13b 26 uint8_t nextWaypoint;
stearnsc 4:c6daeab4a13b 27 bool routeSet = false;
stearnsc 4:c6daeab4a13b 28
stearnsc 4:c6daeab4a13b 29 void readSerial(Serial &s, char str[], int size)
stearnsc 4:c6daeab4a13b 30 {
stearnsc 4:c6daeab4a13b 31 for (int i = 0; i < size; i++) {
stearnsc 4:c6daeab4a13b 32 str[i] = s.getc();
stearnsc 0:9c001c4e7bf4 33 }
stearnsc 0:9c001c4e7bf4 34 }
stearnsc 0:9c001c4e7bf4 35
stearnsc 0:9c001c4e7bf4 36 //sends: "$<command>*<checksum>\r\l"
stearnsc 4:c6daeab4a13b 37 void sendGpsCommand(string command)
stearnsc 4:c6daeab4a13b 38 {
stearnsc 0:9c001c4e7bf4 39 uint8_t checksum = 0;
stearnsc 0:9c001c4e7bf4 40 pc.printf("Sending command to gps: ");
stearnsc 0:9c001c4e7bf4 41 gps.putc('$');
stearnsc 0:9c001c4e7bf4 42 pc.putc('$');
stearnsc 0:9c001c4e7bf4 43 char c;
stearnsc 4:c6daeab4a13b 44 for (int i = 0; i < command.length(); i++) {
stearnsc 0:9c001c4e7bf4 45 c = command[i];
stearnsc 0:9c001c4e7bf4 46 checksum ^= c;
stearnsc 0:9c001c4e7bf4 47 gps.putc(c);
stearnsc 0:9c001c4e7bf4 48 pc.putc(c);
stearnsc 0:9c001c4e7bf4 49 }
stearnsc 0:9c001c4e7bf4 50 gps.putc('*');
stearnsc 0:9c001c4e7bf4 51 pc.putc('*');
stearnsc 0:9c001c4e7bf4 52 string checkSumString;
stearnsc 4:c6daeab4a13b 53 while (checksum > 0) {
stearnsc 0:9c001c4e7bf4 54 uint8_t checksumChar = checksum & 0x0F;
stearnsc 4:c6daeab4a13b 55 if (checksumChar >= 10) {
stearnsc 0:9c001c4e7bf4 56 checksumChar -= 10;
stearnsc 4:c6daeab4a13b 57 checksumChar += 'A';
stearnsc 0:9c001c4e7bf4 58 } else {
stearnsc 4:c6daeab4a13b 59 checksumChar += '0';
stearnsc 0:9c001c4e7bf4 60 }
stearnsc 0:9c001c4e7bf4 61 checkSumString.push_back((char) checksumChar);
stearnsc 4:c6daeab4a13b 62 checksum = checksum >> 4;
stearnsc 0:9c001c4e7bf4 63 }
stearnsc 0:9c001c4e7bf4 64
stearnsc 4:c6daeab4a13b 65 for (int i = checkSumString.length() - 1; i >= 0; i--) {
stearnsc 0:9c001c4e7bf4 66 gps.putc(checkSumString[i]);
stearnsc 4:c6daeab4a13b 67 pc.putc(checkSumString[i]);
stearnsc 4:c6daeab4a13b 68 }
stearnsc 0:9c001c4e7bf4 69 gps.putc('\r');
stearnsc 0:9c001c4e7bf4 70 pc.putc('\r');
stearnsc 0:9c001c4e7bf4 71 gps.putc('\n');
stearnsc 0:9c001c4e7bf4 72 pc.putc('\n');
stearnsc 0:9c001c4e7bf4 73 }
stearnsc 0:9c001c4e7bf4 74
stearnsc 4:c6daeab4a13b 75 long parseDec(string s)
stearnsc 4:c6daeab4a13b 76 {
stearnsc 0:9c001c4e7bf4 77 int mult = 1;
stearnsc 0:9c001c4e7bf4 78 int result = 0;
stearnsc 4:c6daeab4a13b 79 for (int i = s.length() - 1; i >=0; i--) {
stearnsc 4:c6daeab4a13b 80 if (s[i] != '.') {
stearnsc 0:9c001c4e7bf4 81 result += (s[i] - '0') * mult;
stearnsc 0:9c001c4e7bf4 82 mult *= 10;
stearnsc 0:9c001c4e7bf4 83 }
stearnsc 0:9c001c4e7bf4 84 }
stearnsc 0:9c001c4e7bf4 85 return result;
stearnsc 0:9c001c4e7bf4 86 }
stearnsc 0:9c001c4e7bf4 87
stearnsc 0:9c001c4e7bf4 88 //cs: little endian parsing
stearnsc 4:c6daeab4a13b 89 int nextInt(char *data, int i)
stearnsc 4:c6daeab4a13b 90 {
stearnsc 4:c6daeab4a13b 91 i |= data[i];
stearnsc 4:c6daeab4a13b 92 i |= (data[i + 1] << 8);
stearnsc 4:c6daeab4a13b 93 i |= (data[i + 2] << 16);
stearnsc 4:c6daeab4a13b 94 i |= (data[i + 3] << 24);
stearnsc 4:c6daeab4a13b 95 return i;
stearnsc 0:9c001c4e7bf4 96 }
stearnsc 0:9c001c4e7bf4 97
stearnsc 4:c6daeab4a13b 98 void handleXbeeGps()
stearnsc 4:c6daeab4a13b 99 {
stearnsc 0:9c001c4e7bf4 100 static bool reading = false;
stearnsc 0:9c001c4e7bf4 101 static char packet[16];
stearnsc 0:9c001c4e7bf4 102 static int i = 0;
stearnsc 4:c6daeab4a13b 103
stearnsc 0:9c001c4e7bf4 104 char c = xbee.getc();
stearnsc 4:c6daeab4a13b 105 if (reading) {
stearnsc 0:9c001c4e7bf4 106 packet[i] = c;
stearnsc 0:9c001c4e7bf4 107 i++;
stearnsc 4:c6daeab4a13b 108 if (i == 16) {
stearnsc 0:9c001c4e7bf4 109 i = 0;
stearnsc 0:9c001c4e7bf4 110 otherGps.latitude = nextInt(packet, 0);
stearnsc 0:9c001c4e7bf4 111 otherGps.longitude = nextInt(packet, 4);
stearnsc 0:9c001c4e7bf4 112 otherGps.altitude = nextInt(packet, 8);
stearnsc 0:9c001c4e7bf4 113 otherGps.time = nextInt(packet, 12);
stearnsc 4:c6daeab4a13b 114
stearnsc 0:9c001c4e7bf4 115 pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 4:c6daeab4a13b 116 otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time
stearnsc 4:c6daeab4a13b 117 );
stearnsc 0:9c001c4e7bf4 118 reading = false;
stearnsc 0:9c001c4e7bf4 119 }
stearnsc 4:c6daeab4a13b 120 } else if (c == 'X') {
stearnsc 4:c6daeab4a13b 121 reading = true;
stearnsc 0:9c001c4e7bf4 122 }
stearnsc 0:9c001c4e7bf4 123 }
stearnsc 0:9c001c4e7bf4 124
stearnsc 4:c6daeab4a13b 125 void handleGpsData()
stearnsc 4:c6daeab4a13b 126 {
stearnsc 4:c6daeab4a13b 127 static bool reading = false;
stearnsc 4:c6daeab4a13b 128 static stringstream line;
stearnsc 4:c6daeab4a13b 129
stearnsc 4:c6daeab4a13b 130 char c = gps.getc();
stearnsc 4:c6daeab4a13b 131
stearnsc 4:c6daeab4a13b 132 if (reading) {
stearnsc 4:c6daeab4a13b 133 if (c == '*') { //sentence buffer complete; we're ignoring the checksum
stearnsc 0:9c001c4e7bf4 134 string field;
stearnsc 0:9c001c4e7bf4 135 std::getline(line, field, ','); //GPGGA
stearnsc 0:9c001c4e7bf4 136 std::getline(line, field, ','); //time
stearnsc 0:9c001c4e7bf4 137 gpsData.time = parseDec(field);
stearnsc 0:9c001c4e7bf4 138 std::getline(line, field, ','); //latitude
stearnsc 0:9c001c4e7bf4 139 gpsData.latitude = parseDec(field);
stearnsc 0:9c001c4e7bf4 140 std::getline(line, field, ','); //N or S
stearnsc 0:9c001c4e7bf4 141 std::getline(line, field, ','); //longitude
stearnsc 0:9c001c4e7bf4 142 gpsData.longitude = parseDec(field);
stearnsc 0:9c001c4e7bf4 143 std::getline(line, field, ','); //E or W
stearnsc 4:c6daeab4a13b 144 std::getline(line, field, ','); //skip
stearnsc 0:9c001c4e7bf4 145 std::getline(line, field, ','); //skip
stearnsc 0:9c001c4e7bf4 146 std::getline(line, field, ','); //skip
stearnsc 0:9c001c4e7bf4 147 std::getline(line, field, ','); //altitude
stearnsc 0:9c001c4e7bf4 148 gpsData.altitude = parseDec(field);
stearnsc 4:c6daeab4a13b 149
stearnsc 0:9c001c4e7bf4 150 //update whatever needs updating when gps updates
stearnsc 0:9c001c4e7bf4 151 pc.printf("My GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 4:c6daeab4a13b 152 gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time
stearnsc 4:c6daeab4a13b 153 );
stearnsc 4:c6daeab4a13b 154
stearnsc 0:9c001c4e7bf4 155 line.str(string(""));
stearnsc 0:9c001c4e7bf4 156 reading = false;
stearnsc 4:c6daeab4a13b 157
stearnsc 0:9c001c4e7bf4 158 } else {
stearnsc 4:c6daeab4a13b 159 line.put(c);
stearnsc 0:9c001c4e7bf4 160 }
stearnsc 4:c6daeab4a13b 161
stearnsc 4:c6daeab4a13b 162 } else if (c == '$') {
stearnsc 0:9c001c4e7bf4 163 reading = true;
stearnsc 4:c6daeab4a13b 164 }
stearnsc 4:c6daeab4a13b 165 }
stearnsc 4:c6daeab4a13b 166
stearnsc 4:c6daeab4a13b 167 uint16_t calcChecksum(char *line, int size){
stearnsc 4:c6daeab4a13b 168 //TODO implement
stearnsc 4:c6daeab4a13b 169 return 0;
stearnsc 4:c6daeab4a13b 170 }
stearnsc 4:c6daeab4a13b 171
stearnsc 4:c6daeab4a13b 172 bool setRouteIfValid(char routeLine[])
stearnsc 4:c6daeab4a13b 173 {
stearnsc 4:c6daeab4a13b 174 int lineLen = sizeof(routeLine);
stearnsc 4:c6daeab4a13b 175
stearnsc 4:c6daeab4a13b 176 uint16_t checksum = routeLine[lineLen - 1];
stearnsc 4:c6daeab4a13b 177 checksum |= routeLine[lineLen - 2] << 8;
stearnsc 4:c6daeab4a13b 178 if (calcChecksum(routeLine, lineLen - 2) != checksum) return false; //checksum invalid
stearnsc 4:c6daeab4a13b 179
stearnsc 4:c6daeab4a13b 180 routeLength = routeLine[0];
stearnsc 4:c6daeab4a13b 181 int waypointIndex = 0;
stearnsc 4:c6daeab4a13b 182 int lineIndex = 1; //start reading at first coordinate
stearnsc 4:c6daeab4a13b 183 for (int i = 0; i < routeLength; i++) {
stearnsc 4:c6daeab4a13b 184 route[waypointIndex].latitude = 0;
stearnsc 4:c6daeab4a13b 185 route[waypointIndex].longitude = 0;
stearnsc 4:c6daeab4a13b 186 route[waypointIndex].latitude |= routeLine[lineIndex];
stearnsc 4:c6daeab4a13b 187 route[waypointIndex].latitude |= routeLine[lineIndex + 1] << 8;
stearnsc 4:c6daeab4a13b 188 route[waypointIndex].latitude |= routeLine[lineIndex + 2] << 16;
stearnsc 4:c6daeab4a13b 189 route[waypointIndex].longitude |= routeLine[lineIndex + 3];
stearnsc 4:c6daeab4a13b 190 route[waypointIndex].longitude |= routeLine[lineIndex + 4] << 8;
stearnsc 4:c6daeab4a13b 191 route[waypointIndex].longitude |= routeLine[lineIndex + 5] << 16;
stearnsc 4:c6daeab4a13b 192 lineIndex += 6;
stearnsc 4:c6daeab4a13b 193 }
stearnsc 4:c6daeab4a13b 194 return true;
stearnsc 0:9c001c4e7bf4 195 }
stearnsc 0:9c001c4e7bf4 196
stearnsc 4:c6daeab4a13b 197 //route data format: 'R' numWaypoints Latitude Longitude (repeat numWaypoints times, 6 bytes per waypoint)
stearnsc 4:c6daeab4a13b 198 // byte indices: 1 2 3 4 5 6 7 8 (....)
stearnsc 4:c6daeab4a13b 199 bool handleRouteData(char byte)
stearnsc 4:c6daeab4a13b 200 {
stearnsc 4:c6daeab4a13b 201 if (routeSet) return false; //never allow setting route twice, as can cause bad behavior during flight
stearnsc 4:c6daeab4a13b 202
stearnsc 4:c6daeab4a13b 203 static bool reading = false;
stearnsc 4:c6daeab4a13b 204 static char *routeLine;
stearnsc 4:c6daeab4a13b 205 static uint8_t numWaypoints;
stearnsc 4:c6daeab4a13b 206 static uint8_t i = 0;
stearnsc 4:c6daeab4a13b 207
stearnsc 4:c6daeab4a13b 208 if (!reading) {
stearnsc 4:c6daeab4a13b 209 numWaypoints = byte;
stearnsc 4:c6daeab4a13b 210 routeLine = (char*) malloc(1 + 6 * numWaypoints + 2); //first byte is number of waypoints, each of which is 6 bytes. Last 2 are checksum
stearnsc 4:c6daeab4a13b 211 // route size is kept in line for checksum purposes.
stearnsc 4:c6daeab4a13b 212 reading = true;
stearnsc 4:c6daeab4a13b 213 }
stearnsc 4:c6daeab4a13b 214
stearnsc 4:c6daeab4a13b 215 routeLine[i] = byte;
stearnsc 4:c6daeab4a13b 216 i++;
stearnsc 4:c6daeab4a13b 217 if (i == numWaypoints) {
stearnsc 4:c6daeab4a13b 218 bool validLine = setRouteIfValid(routeLine);
stearnsc 4:c6daeab4a13b 219 if (validLine) routeSet = true;
stearnsc 4:c6daeab4a13b 220 free(routeLine);
stearnsc 4:c6daeab4a13b 221 reading = false;
stearnsc 4:c6daeab4a13b 222 return false;
stearnsc 4:c6daeab4a13b 223 } else {
stearnsc 4:c6daeab4a13b 224 return true;
stearnsc 4:c6daeab4a13b 225 }
stearnsc 4:c6daeab4a13b 226 }
stearnsc 4:c6daeab4a13b 227
stearnsc 4:c6daeab4a13b 228 void handleComm()
stearnsc 4:c6daeab4a13b 229 {
stearnsc 4:c6daeab4a13b 230 static bool reading = false;
stearnsc 4:c6daeab4a13b 231 static bool (*handleCommData) (char);
stearnsc 4:c6daeab4a13b 232 //cs: handleCommData is a pointer to whatever the current "read communications" function is. It returns a boolean indicating whether
stearnsc 4:c6daeab4a13b 233 // there is more data to be read.
stearnsc 4:c6daeab4a13b 234 if (reading) {
stearnsc 4:c6daeab4a13b 235 reading = handleCommData(xbee.getc());
stearnsc 4:c6daeab4a13b 236 } else {
stearnsc 4:c6daeab4a13b 237 switch (xbee.getc()) {
stearnsc 4:c6daeab4a13b 238 case 'R': //route data
stearnsc 4:c6daeab4a13b 239 handleCommData = handleRouteData;
stearnsc 4:c6daeab4a13b 240 reading = true;
stearnsc 4:c6daeab4a13b 241 break;
stearnsc 4:c6daeab4a13b 242 default:
stearnsc 4:c6daeab4a13b 243 break;
stearnsc 4:c6daeab4a13b 244 }
stearnsc 4:c6daeab4a13b 245 }
stearnsc 4:c6daeab4a13b 246 }
stearnsc 4:c6daeab4a13b 247
stearnsc 4:c6daeab4a13b 248 int main()
stearnsc 4:c6daeab4a13b 249 {
stearnsc 0:9c001c4e7bf4 250 gps.baud(57600);
stearnsc 0:9c001c4e7bf4 251 xbee.baud(9600);
stearnsc 0:9c001c4e7bf4 252 pc.baud(57600);
stearnsc 4:c6daeab4a13b 253
stearnsc 5:7bc8bec23b03 254 sendGpsCommand("PMTK301,1");
stearnsc 4:c6daeab4a13b 255 while(true) {
stearnsc 4:c6daeab4a13b 256 xbee.putc(gps.getc());
stearnsc 4:c6daeab4a13b 257 }
stearnsc 4:c6daeab4a13b 258 // gps.attach(&handleGpsData, Serial::RxIrq);
stearnsc 0:9c001c4e7bf4 259 // xbee.attach(&handleXbeeGps, Serial::RxIrq);
stearnsc 0:9c001c4e7bf4 260 }