Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
Diff: main.cpp
- Revision:
- 8:28b866df62cf
- Parent:
- 7:c75d5e5e6bfc
- Child:
- 9:da906eeac51e
- Child:
- 17:323fc40376d5
--- a/main.cpp Tue Apr 01 15:52:08 2014 +0000 +++ b/main.cpp Tue Apr 01 17:58:35 2014 +0000 @@ -3,7 +3,7 @@ #include <sstream> #include "adapt/usb.h" #include "adapt/camera.h" -/* + Serial pc(USBTX,USBRX); Serial xbee(p9,p10);//tx, rx Serial gps(p28,p27); @@ -16,23 +16,22 @@ int time; //in milliseconds } GpsData; -GpsData gpsData; -GpsData otherGps; - -void readSerial(Serial &s, char str[], int size){ - for (int i = 0; i < size; i++){ - str[i] = s.getc(); +void readSerial(Serial &s, char str[], int size) +{ + for (int i = 0; i < size; i++) { + str[i] = s.getc(); } } //sends: "$<command>*<checksum>\r\l" -void sendGpsCommand(string command){ +void sendGpsCommand(string command) +{ uint8_t checksum = 0; pc.printf("Sending command to gps: "); gps.putc('$'); pc.putc('$'); char c; - for (int i = 0; i < command.length(); i++){ + for (int i = 0; i < command.length(); i++) { c = command[i]; checksum ^= c; gps.putc(c); @@ -41,149 +40,81 @@ gps.putc('*'); pc.putc('*'); string checkSumString; - while (checksum > 0){ + while (checksum > 0) { uint8_t checksumChar = checksum & 0x0F; - if (checksumChar >= 10){ + if (checksumChar >= 10) { checksumChar -= 10; - checksumChar += 'A'; + checksumChar += 'A'; } else { - checksumChar += '0'; + checksumChar += '0'; } checkSumString.push_back((char) checksumChar); - checksum = checksum >> 4; + checksum = checksum >> 4; } - for (int i = checkSumString.length() - 1; i >= 0; i--){ + for (int i = checkSumString.length() - 1; i >= 0; i--) { gps.putc(checkSumString[i]); - pc.putc(checkSumString[i]); - } + pc.putc(checkSumString[i]); + } gps.putc('\r'); pc.putc('\r'); gps.putc('\n'); pc.putc('\n'); } - -long parseDec(string s){ - int mult = 1; - int result = 0; - for (int i = s.length() - 1; i >=0; i--){ - if (s[i] != '.'){ - result += (s[i] - '0') * mult; - mult *= 10; - } - } - return result; -} - -//cs: little endian parsing -int nextInt(char *data, int i){ - i |= data[i]; - i |= (data[i + 1] << 8); - i |= (data[i + 2] << 16); - i |= (data[i + 3] << 24); - return i; -} +// +////cs: little endian parsing +//int nextInt(char *data, int i) +//{ +// i |= data[i]; +// i |= (data[i + 1] << 8); +// i |= (data[i + 2] << 16); +// i |= (data[i + 3] << 24); +// return i; +//} -void handleXbeeGps(){ - static bool reading = false; - static char packet[16]; - static int i = 0; - - char c = xbee.getc(); - if (reading){ - packet[i] = c; - i++; - if (i == 16){ - i = 0; - otherGps.latitude = nextInt(packet, 0); - otherGps.longitude = nextInt(packet, 4); - otherGps.altitude = nextInt(packet, 8); - otherGps.time = nextInt(packet, 12); - - pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n", - otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time - ); - reading = false; - } - } else if (c == 'X'){ - reading = true; - } -} - -void handleGpsData(){ - static bool reading = false; - static stringstream line; - - char c = gps.getc(); - - if (reading){ - if (c == '*'){ //sentence buffer complete; we're ignoring the checksum - string field; - std::getline(line, field, ','); //GPGGA - std::getline(line, field, ','); //time - gpsData.time = parseDec(field); - std::getline(line, field, ','); //latitude - gpsData.latitude = parseDec(field); - std::getline(line, field, ','); //N or S - std::getline(line, field, ','); //longitude - gpsData.longitude = parseDec(field); - std::getline(line, field, ','); //E or W - std::getline(line, field, ','); //skip - std::getline(line, field, ','); //skip - std::getline(line, field, ','); //skip - std::getline(line, field, ','); //altitude - gpsData.altitude = parseDec(field); - - //update whatever needs updating when gps updates - pc.printf("My GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n", - gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time - ); - - line.str(string("")); - reading = false; - - } else { - line.put(c); - } - - } else if (c == '$') { - reading = true; - } -} +//void handleXbeeGps() +//{ +// static bool reading = false; +// static char packet[16]; +// static int i = 0; +// +// char c = xbee.getc(); +// if (reading) { +// packet[i] = c; +// i++; +// if (i == 16) { +// i = 0; +// otherGps.latitude = nextInt(packet, 0); +// otherGps.longitude = nextInt(packet, 4); +// otherGps.altitude = nextInt(packet, 8); +// otherGps.time = nextInt(packet, 12); +// +// pc.printf("His GPS data: Lat: %d, Lon: %d, Alt: %d, Time:%d\r\n", +// otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time +// ); +// reading = false; +// } +// } else if (c == 'X') { +// reading = true; +// } +//} -*/ -Serial gps(p28,p27); - -int main(){ +int main() +{ ImageHandle imageHand; GPSHand gpsHand; - + /// Main Loop - while(1){ - // Run image handler - imageHand.run(); - // Run GPS handler - gpsHand.run(); + while(true) { + gps.baud(57600); + xbee.baud(9600); + pc.baud(57600); + + sendGpsCommand("PMTK301,1"); + while(true) { + pc.putc(gps.getc()); + } + gps.attach(&handleGpsData, Serial::RxIrq); + xbee.attach(&handleXbeeGps, Serial::RxIrq)//; } - - USB::getSerial().printf("Start!\n"); - gps.baud(57600); - while(true){ - if(gps.readable()>0){ - char c = gps.getc(); - USB::getSerial().printf("%c",c); - } - } - //USB::getSerial().printf("Done(%d)\n",i); - while(1){ - } -// gps.baud(57600); -// xbee.baud(9600); -// pc.baud(57600); - -// sendGpsCommand("PMTK301,1"); -// while(true){pc.putc(gps.getc());} -// gps.attach(&handleGpsData, Serial::RxIrq); -// xbee.attach(&handleXbeeGps, Serial::RxIrq); -}