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);
-}