Colin Stearns / Mbed 2 deprecated dgps

WIP for dgps

Dependencies:   mbed

main.cpp@6:f0248eb6714d, 2014-04-03 (annotated)

Committer:
dylanembed123
Date:
Thu Apr 03 16:27:23 2014 +0000
Revision:
6:f0248eb6714d
Parent:
4:c75d5e5e6bfc 
Fix compile errors;

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>
dylanembed123 4:c75d5e5e6bfc 4 #include "adapt/usb.h"
dylanembed123 4:c75d5e5e6bfc 5 #include "adapt/camera.h"
dylanembed123 6:f0248eb6714d 6 #include "handle/handleCamera.h"
dylanembed123 6:f0248eb6714d 7 #include "handle/handleGPS.h"
dylanembed123 3:434d20e99e49 8 /*
stearnsc 0:9c001c4e7bf4 9 Serial pc(USBTX,USBRX);
stearnsc 0:9c001c4e7bf4 10 Serial xbee(p9,p10);//tx, rx
stearnsc 0:9c001c4e7bf4 11 Serial gps(p28,p27);
dylanembed123 3:434d20e99e49 12 Serial camera(p13,p14);
stearnsc 0:9c001c4e7bf4 13
stearnsc 0:9c001c4e7bf4 14 typedef struct {
stearnsc 0:9c001c4e7bf4 15 int latitude; //in .0001 minutes
stearnsc 0:9c001c4e7bf4 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 0:9c001c4e7bf4 24 void readSerial(Serial &s, char str[], int size){
stearnsc 0:9c001c4e7bf4 25 for (int i = 0; i < size; i++){
stearnsc 0:9c001c4e7bf4 26 str[i] = s.getc();
stearnsc 0:9c001c4e7bf4 27 }
stearnsc 0:9c001c4e7bf4 28 }
stearnsc 0:9c001c4e7bf4 29
stearnsc 0:9c001c4e7bf4 30 //sends: "$<command>*<checksum>\r\l"
stearnsc 0:9c001c4e7bf4 31 void sendGpsCommand(string command){
stearnsc 0:9c001c4e7bf4 32 uint8_t checksum = 0;
stearnsc 0:9c001c4e7bf4 33 pc.printf("Sending command to gps: ");
stearnsc 0:9c001c4e7bf4 34 gps.putc('$');
stearnsc 0:9c001c4e7bf4 35 pc.putc('$');
stearnsc 0:9c001c4e7bf4 36 char c;
stearnsc 0:9c001c4e7bf4 37 for (int i = 0; i < command.length(); i++){
stearnsc 0:9c001c4e7bf4 38 c = command[i];
stearnsc 0:9c001c4e7bf4 39 checksum ^= c;
stearnsc 0:9c001c4e7bf4 40 gps.putc(c);
stearnsc 0:9c001c4e7bf4 41 pc.putc(c);
stearnsc 0:9c001c4e7bf4 42 }
stearnsc 0:9c001c4e7bf4 43 gps.putc('*');
stearnsc 0:9c001c4e7bf4 44 pc.putc('*');
stearnsc 0:9c001c4e7bf4 45 string checkSumString;
stearnsc 0:9c001c4e7bf4 46 while (checksum > 0){
stearnsc 0:9c001c4e7bf4 47 uint8_t checksumChar = checksum & 0x0F;
stearnsc 0:9c001c4e7bf4 48 if (checksumChar >= 10){
stearnsc 0:9c001c4e7bf4 49 checksumChar -= 10;
stearnsc 0:9c001c4e7bf4 50 checksumChar += 'A';
stearnsc 0:9c001c4e7bf4 51 } else {
stearnsc 0:9c001c4e7bf4 52 checksumChar += '0';
stearnsc 0:9c001c4e7bf4 53 }
stearnsc 0:9c001c4e7bf4 54 checkSumString.push_back((char) checksumChar);
stearnsc 0:9c001c4e7bf4 55 checksum = checksum >> 4;
stearnsc 0:9c001c4e7bf4 56 }
stearnsc 0:9c001c4e7bf4 57
stearnsc 0:9c001c4e7bf4 58 for (int i = checkSumString.length() - 1; i >= 0; i--){
stearnsc 0:9c001c4e7bf4 59 gps.putc(checkSumString[i]);
stearnsc 0:9c001c4e7bf4 60 pc.putc(checkSumString[i]);
stearnsc 0:9c001c4e7bf4 61 }
stearnsc 0:9c001c4e7bf4 62 gps.putc('\r');
stearnsc 0:9c001c4e7bf4 63 pc.putc('\r');
stearnsc 0:9c001c4e7bf4 64 gps.putc('\n');
stearnsc 0:9c001c4e7bf4 65 pc.putc('\n');
stearnsc 0:9c001c4e7bf4 66 }
stearnsc 0:9c001c4e7bf4 67
stearnsc 0:9c001c4e7bf4 68 long parseDec(string s){
stearnsc 0:9c001c4e7bf4 69 int mult = 1;
stearnsc 0:9c001c4e7bf4 70 int result = 0;
stearnsc 0:9c001c4e7bf4 71 for (int i = s.length() - 1; i >=0; i--){
stearnsc 0:9c001c4e7bf4 72 if (s[i] != '.'){
stearnsc 0:9c001c4e7bf4 73 result += (s[i] - '0') * mult;
stearnsc 0:9c001c4e7bf4 74 mult *= 10;
stearnsc 0:9c001c4e7bf4 75 }
stearnsc 0:9c001c4e7bf4 76 }
stearnsc 0:9c001c4e7bf4 77 return result;
stearnsc 0:9c001c4e7bf4 78 }
stearnsc 0:9c001c4e7bf4 79
stearnsc 0:9c001c4e7bf4 80 //cs: little endian parsing
stearnsc 0:9c001c4e7bf4 81 int nextInt(char *data, int i){
stearnsc 0:9c001c4e7bf4 82 i |= data[i];
stearnsc 0:9c001c4e7bf4 83 i |= (data[i + 1] << 8);
stearnsc 0:9c001c4e7bf4 84 i |= (data[i + 2] << 16);
stearnsc 0:9c001c4e7bf4 85 i |= (data[i + 3] << 24);
stearnsc 0:9c001c4e7bf4 86 return i;
stearnsc 0:9c001c4e7bf4 87 }
stearnsc 0:9c001c4e7bf4 88
stearnsc 0:9c001c4e7bf4 89 void handleXbeeGps(){
stearnsc 0:9c001c4e7bf4 90 static bool reading = false;
stearnsc 0:9c001c4e7bf4 91 static char packet[16];
stearnsc 0:9c001c4e7bf4 92 static int i = 0;
stearnsc 0:9c001c4e7bf4 93
stearnsc 0:9c001c4e7bf4 94 char c = xbee.getc();
stearnsc 0:9c001c4e7bf4 95 if (reading){
stearnsc 0:9c001c4e7bf4 96 packet[i] = c;
stearnsc 0:9c001c4e7bf4 97 i++;
stearnsc 0:9c001c4e7bf4 98 if (i == 16){
stearnsc 0:9c001c4e7bf4 99 i = 0;
stearnsc 0:9c001c4e7bf4 100 otherGps.latitude = nextInt(packet, 0);
stearnsc 0:9c001c4e7bf4 101 otherGps.longitude = nextInt(packet, 4);
stearnsc 0:9c001c4e7bf4 102 otherGps.altitude = nextInt(packet, 8);
stearnsc 0:9c001c4e7bf4 103 otherGps.time = nextInt(packet, 12);
stearnsc 0:9c001c4e7bf4 104
stearnsc 0:9c001c4e7bf4 105 pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 0:9c001c4e7bf4 106 otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time
stearnsc 0:9c001c4e7bf4 107 );
stearnsc 0:9c001c4e7bf4 108 reading = false;
stearnsc 0:9c001c4e7bf4 109 }
stearnsc 0:9c001c4e7bf4 110 } else if (c == 'X'){
stearnsc 0:9c001c4e7bf4 111 reading = true;
stearnsc 0:9c001c4e7bf4 112 }
stearnsc 0:9c001c4e7bf4 113 }
stearnsc 0:9c001c4e7bf4 114
stearnsc 0:9c001c4e7bf4 115 void handleGpsData(){
stearnsc 0:9c001c4e7bf4 116 static bool reading = false;
stearnsc 0:9c001c4e7bf4 117 static stringstream line;
stearnsc 0:9c001c4e7bf4 118
stearnsc 0:9c001c4e7bf4 119 char c = gps.getc();
stearnsc 0:9c001c4e7bf4 120
stearnsc 0:9c001c4e7bf4 121 if (reading){
stearnsc 0:9c001c4e7bf4 122 if (c == '*'){ //sentence buffer complete; we're ignoring the checksum
stearnsc 0:9c001c4e7bf4 123 string field;
stearnsc 0:9c001c4e7bf4 124 std::getline(line, field, ','); //GPGGA
stearnsc 0:9c001c4e7bf4 125 std::getline(line, field, ','); //time
stearnsc 0:9c001c4e7bf4 126 gpsData.time = parseDec(field);
stearnsc 0:9c001c4e7bf4 127 std::getline(line, field, ','); //latitude
stearnsc 0:9c001c4e7bf4 128 gpsData.latitude = parseDec(field);
stearnsc 0:9c001c4e7bf4 129 std::getline(line, field, ','); //N or S
stearnsc 0:9c001c4e7bf4 130 std::getline(line, field, ','); //longitude
stearnsc 0:9c001c4e7bf4 131 gpsData.longitude = parseDec(field);
stearnsc 0:9c001c4e7bf4 132 std::getline(line, field, ','); //E or W
stearnsc 0:9c001c4e7bf4 133 std::getline(line, field, ','); //skip
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, ','); //altitude
stearnsc 0:9c001c4e7bf4 137 gpsData.altitude = parseDec(field);
stearnsc 0:9c001c4e7bf4 138
stearnsc 0:9c001c4e7bf4 139 //update whatever needs updating when gps updates
stearnsc 0:9c001c4e7bf4 140 pc.printf("My GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n",
stearnsc 0:9c001c4e7bf4 141 gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time
stearnsc 0:9c001c4e7bf4 142 );
stearnsc 0:9c001c4e7bf4 143
stearnsc 0:9c001c4e7bf4 144 line.str(string(""));
stearnsc 0:9c001c4e7bf4 145 reading = false;
stearnsc 0:9c001c4e7bf4 146
stearnsc 0:9c001c4e7bf4 147 } else {
stearnsc 0:9c001c4e7bf4 148 line.put(c);
stearnsc 0:9c001c4e7bf4 149 }
stearnsc 0:9c001c4e7bf4 150
stearnsc 0:9c001c4e7bf4 151 } else if (c == '$') {
stearnsc 0:9c001c4e7bf4 152 reading = true;
stearnsc 0:9c001c4e7bf4 153 }
stearnsc 0:9c001c4e7bf4 154 }
dylanembed123 4:c75d5e5e6bfc 155
dylanembed123 4:c75d5e5e6bfc 156
dylanembed123 3:434d20e99e49 157 */
dylanembed123 4:c75d5e5e6bfc 158 Serial gps(p28,p27);
dylanembed123 4:c75d5e5e6bfc 159
stearnsc 0:9c001c4e7bf4 160 int main(){
dylanembed123 4:c75d5e5e6bfc 161 ImageHandle imageHand;
dylanembed123 6:f0248eb6714d 162 GPSHandle gpsHand;
dylanembed123 4:c75d5e5e6bfc 163
dylanembed123 4:c75d5e5e6bfc 164 /// Main Loop
dylanembed123 4:c75d5e5e6bfc 165 while(1){
dylanembed123 4:c75d5e5e6bfc 166 // Run image handler
dylanembed123 4:c75d5e5e6bfc 167 imageHand.run();
dylanembed123 4:c75d5e5e6bfc 168 // Run GPS handler
dylanembed123 4:c75d5e5e6bfc 169 gpsHand.run();
dylanembed123 3:434d20e99e49 170 }
dylanembed123 4:c75d5e5e6bfc 171
dylanembed123 4:c75d5e5e6bfc 172 USB::getSerial().printf("Start!\n");
dylanembed123 4:c75d5e5e6bfc 173 gps.baud(57600);
dylanembed123 4:c75d5e5e6bfc 174 while(true){
dylanembed123 4:c75d5e5e6bfc 175 if(gps.readable()>0){
dylanembed123 4:c75d5e5e6bfc 176 char c = gps.getc();
dylanembed123 4:c75d5e5e6bfc 177 USB::getSerial().printf("%c",c);
dylanembed123 4:c75d5e5e6bfc 178 }
dylanembed123 3:434d20e99e49 179 }
dylanembed123 3:434d20e99e49 180 //USB::getSerial().printf("Done(%d)\n",i);
dylanembed123 3:434d20e99e49 181 while(1){
dylanembed123 3:434d20e99e49 182 }
dylanembed123 3:434d20e99e49 183 // gps.baud(57600);
dylanembed123 3:434d20e99e49 184 // xbee.baud(9600);
dylanembed123 3:434d20e99e49 185 // pc.baud(57600);
stearnsc 0:9c001c4e7bf4 186
stearnsc 0:9c001c4e7bf4 187 // sendGpsCommand("PMTK301,1");
stearnsc 0:9c001c4e7bf4 188 // while(true){pc.putc(gps.getc());}
dylanembed123 3:434d20e99e49 189 // gps.attach(&handleGpsData, Serial::RxIrq);
stearnsc 0:9c001c4e7bf4 190 // xbee.attach(&handleXbeeGps, Serial::RxIrq);
stearnsc 0:9c001c4e7bf4 191 }