Revision 4:c6daeab4a13b, committed 2014-03-10

Comitter:

stearnsc

Date:

Mon Mar 10 00:57:48 2014 +0000

Parent:

3:87c4cd4cb752

Child:

5:7bc8bec23b03

Commit message:

cs: route setting subroutine (untested)

Changed in this revision

--- a/main.cpp	Sun Feb 16 22:12:08 2014 +0000
+++ b/main.cpp	Mon Mar 10 00:57:48 2014 +0000
@@ -9,6 +9,11 @@
 typedef struct {
     int latitude;  //in .0001 minutes
     int longitude; //in .0001 minutes
+} GpsCoordinates;
+
+typedef struct {
+    int latitude;  //in .0001 minutes
+    int longitude; //in .0001 minutes
     int altitude;  //in decimeters
     int time;      //in milliseconds
 } GpsData;
@@ -16,20 +21,27 @@
 GpsData gpsData;
 GpsData otherGps;
 
-void readSerial(Serial &s, char str[], int size){
-    for (int i = 0; i < size; i++){
-        str[i] = s.getc();   
+GpsCoordinates route[256];
+uint8_t routeLength;
+uint8_t nextWaypoint;
+bool routeSet = false;
+
+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);
@@ -38,33 +50,34 @@
     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){
+long parseDec(string s)
+{
     int mult = 1;
     int result = 0;
-    for (int i = s.length() - 1; i >=0; i--){
-        if (s[i] != '.'){
+    for (int i = s.length() - 1; i >=0; i--) {
+        if (s[i] != '.') {
             result += (s[i] - '0') * mult;
             mult *= 10;
         }
@@ -73,48 +86,51 @@
 }
 
 //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;
+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(){
+void handleXbeeGps()
+{
     static bool reading = false;
     static char packet[16];
     static int i = 0;
-    
+
     char c = xbee.getc();
-    if (reading){
+    if (reading) {
         packet[i] = c;
         i++;
-        if (i == 16){
+        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
-            );
+                      otherGps.latitude, otherGps.longitude, otherGps.altitude, otherGps.time
+                     );
             reading = false;
         }
-    } else if (c == 'X'){
-        reading = true;    
+    } 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
+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
@@ -125,36 +141,120 @@
             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, ','); //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
-            );
-            
+                      gpsData.latitude, gpsData.longitude, gpsData.altitude, gpsData.time
+                     );
+
             line.str(string(""));
             reading = false;
-            
+
         } else {
-            line.put(c);    
+            line.put(c);
         }
-        
-     } else if (c == '$') {
+
+    } else if (c == '$') {
         reading = true;
-     }
+    }
+}
+
+uint16_t calcChecksum(char *line, int size){
+    //TODO implement
+    return 0;    
+}
+
+bool setRouteIfValid(char routeLine[])
+{
+    int lineLen = sizeof(routeLine);
+
+    uint16_t checksum = routeLine[lineLen - 1];
+    checksum |= routeLine[lineLen - 2] << 8;
+    if (calcChecksum(routeLine, lineLen - 2) != checksum) return false; //checksum invalid
+
+    routeLength = routeLine[0];
+    int waypointIndex = 0;
+    int lineIndex = 1; //start reading at first coordinate
+    for (int i = 0; i < routeLength; i++) {
+        route[waypointIndex].latitude = 0;
+        route[waypointIndex].longitude = 0;
+        route[waypointIndex].latitude |= routeLine[lineIndex];
+        route[waypointIndex].latitude |= routeLine[lineIndex + 1] << 8;
+        route[waypointIndex].latitude |= routeLine[lineIndex + 2] << 16;
+        route[waypointIndex].longitude |= routeLine[lineIndex + 3];
+        route[waypointIndex].longitude |= routeLine[lineIndex + 4] << 8;
+        route[waypointIndex].longitude |= routeLine[lineIndex + 5] << 16;
+        lineIndex += 6;
+    }
+    return true;
 }
 
-int main(){
+//route data format: 'R' numWaypoints   Latitude    Longitude  (repeat numWaypoints times, 6 bytes per waypoint)
+// byte indices:      1     2           3  4  5     6  7  8                         (....)
+bool handleRouteData(char byte)
+{
+    if (routeSet) return false; //never allow setting route twice, as can cause bad behavior during flight
+
+    static bool reading = false;
+    static char *routeLine;
+    static uint8_t numWaypoints;
+    static uint8_t i = 0;
+
+    if (!reading) {
+        numWaypoints = byte;
+        routeLine = (char*) malloc(1 + 6 * numWaypoints + 2); //first byte is number of waypoints, each of which is 6 bytes. Last 2 are checksum
+        // route size is kept in line for checksum purposes.
+        reading = true;
+    }
+
+    routeLine[i] = byte;
+    i++;
+    if (i == numWaypoints) {
+        bool validLine = setRouteIfValid(routeLine);
+        if (validLine) routeSet = true;
+        free(routeLine);
+        reading = false;
+        return false;
+    } else {
+        return true;
+    }
+}
+
+void handleComm()
+{
+    static bool reading = false;
+    static bool (*handleCommData) (char);
+    //cs: handleCommData is a pointer to whatever the current "read communications" function is. It returns a boolean indicating whether
+    //      there is more data to be read.
+    if (reading) {
+        reading = handleCommData(xbee.getc());
+    } else {
+        switch (xbee.getc()) {
+            case 'R':   //route data
+                handleCommData = handleRouteData;
+                reading = true;
+                break;
+            default:
+                break;
+        }
+    }
+}
+
+int main()
+{
     gps.baud(57600);
     xbee.baud(9600);
     pc.baud(57600);
-    
+
 //    sendGpsCommand("PMTK301,1");
-//    while(true){pc.putc(gps.getc());}
-    gps.attach(&handleGpsData, Serial::RxIrq);
+    while(true) {
+        xbee.putc(gps.getc());
+    }
+//    gps.attach(&handleGpsData, Serial::RxIrq);
 //    xbee.attach(&handleXbeeGps, Serial::RxIrq);
 }