Diff: main.cpp

Revision:

31:c40f16ff3a2f

Parent:

26:dc00998140af

--- a/main.cpp	Fri Jan 16 17:26:07 2015 +0000
+++ b/main.cpp	Fri Jan 16 20:56:11 2015 +0000
@@ -9,12 +9,13 @@
 #include "Line.h"
 #include "stringUtils.h"
 
+FILE * fp;
+LocalFileSystem local("local"); // Create the local filesystem under the name "local"
 // Connect the TX of the GPS module to p10 RX input
 MODSERIAL b(p9, p10);
 MODSERIAL pc(USBTX, USBRX);
 MODSERIAL bluetooth(p13, p14);
 int checksumm;
-
 int dont = 0;
 #define g0 9.812865328//Gravity at Earth's surface in m/s/s
 #define SAMPLES 8//Number of samples to average.
@@ -194,6 +195,7 @@
     return Point(a.GetX() - b.GetX(),  a.GetY() - b.GetY());
 }
 
+//defining vectore opperation for distance to line
 #define dot(u,v)   ((u).GetX() * (v).GetX()+ (u).GetY() * (v).GetY())
 #define norm(v)     sqrt(dot(v,v))     // norm = length of  vector
 #define d(u,v)      norm(point_sub(u,v))          // distance = norm of difference
@@ -346,7 +348,7 @@
 
 void activate_antenna()
 {
-b.baud(gps_baud);
+    b.baud(gps_baud);
     antenna_active = 1;
 }
 
@@ -549,7 +551,6 @@
                 pc.puts(output);
     char *token;
     int  token_counter = 0;
-    char *cs   = (char *)NULL;
     token = strtok(cs_string, "*");
     while (token) 
     {
@@ -907,30 +908,6 @@
        sprintf(output,"%s\r\n",pc_string);
        bluetooth.puts(output);
     } 
-    else if (!strncmp(pc_string, "$PRINT_VTG=0",12)) 
-    {
-        print_vtg = 0;
-    } 
-    else if (!strncmp(pc_string, "$PRINT_VTG=1",12)) 
-    {
-        print_vtg = 1;
-    } 
-    else if (!strncmp(pc_string, "$PRINT_GSV=0",12))
-    {
-        print_gsv = 0;
-    } 
-    else if (!strncmp(pc_string, "$PRINT_GSV=1",12)) 
-    {
-        print_gsv = 1;
-    } 
-    else if (!strncmp(pc_string, "$PRINT_GSA=0",12)) 
-    {
-        print_gsa = 0;
-    } 
-    else if (!strncmp(pc_string, "$PRINT_GSA=1",12)) 
-    {
-        print_gsa = 1;
-    } 
     else if (!strncmp(pc_string, "$PRINT_EULER=1",14))
     {
         print_euler = 1;
@@ -974,21 +951,6 @@
 
 void gps_analyse(char* gps_string)
 {
-    if ( connecting == 1 ) 
-    {
-        if ( gps_connecting.read_ms() < connect_time && reading == 0 ) 
-        {
-        if ( bluetooth.writeable() > 0 )
-        {
-            bluetooth.puts(gps_string);
-        } 
-        }
-        else 
-        {
-            connecting = 0;
-            gps_connecting.stop();
-        }
-    }
     if (!strncmp(gps_string, "$GPRMC", 6)) 
     {
        // if ( connecting == 0 )// && correct_rmc == 1 ) 
@@ -999,91 +961,79 @@
     } 
     else if (!strncmp(gps_string, "$GPGGA", 6)) 
     {
-      //  if ( connecting == 0 ) 
-      //  {
             bluetooth.puts(gps_string);
-     //   }
-        //nmea_gga(gps_string);//analyse and decompose the gga string
-    } 
-    else if (!strncmp(gps_string, "$GPVTG", 6)) 
-    {
-       // if ( print_vtg == 1 && connecting == 0 ) 
-       // {
-            bluetooth.puts(gps_string);
-      //  }
-    } 
-    else if (!strncmp(gps_string, "$GPGSV", 6)) 
-    {
-        if ( print_gsv == 1 && connecting == 0 ) 
-        {
-            bluetooth.puts(gps_string);
-        }
-    } 
-    else if (!strncmp(gps_string, "$GPGSA", 6)) 
-    {
-        if ( print_gsa == 1 && connecting == 0 ) 
-        {
-            bluetooth.puts(gps_string);
-        }
+        //nmea_gga(gps_string);//analyse and decompose the gga string---no need to use it, just passing to farmerGPS
     } 
     else 
     {
+        bluetooth.puts(gps_string);
     }
 }
 
-/*void rxCallback(MODSERIAL_IRQ_INFO *q)
-{
-    if ( bluetooth.rxGetLastChar() == '\n') 
-    {
-        newline_detected = true;
-    }
-}*/
+int i2 = 0;
+bool end2 = false;
+bool start2 = false;
 
-void getline2()
+bool getline2()
 {
-    for (int i = 0; i<126; i++) 
-    {
-        msg2[i] = bluetooth.getc();
-        if (msg2[i] == '\n') 
+    while (1)
+    {    
+        if( !bluetooth.readable() )
+        {
+            break;
+        }
+        char c = bluetooth.getc();
+        if (c == 36 ){start2=true;end2 = false; i2 = 0;}
+        if (c == 10) {end2=true; start2 = false;}
+        if (start2)
         {
-            msg2[i+1] = 0;
-            dont = 0;
-            return;
+            msg2[i2]=c;
+            i2++;
+            if (i2>255) break; //protect msg buffer!
+        }
+        if (end2)
+        {
+            msg2[i2]=c;   
+            msg2[i2+1] = 0;
+            start2 = false;
+            end2 = true;
+            break;
         }
     }
+    return end2;
 }
 
-void getline()
+    int i=0;
+    bool start=false;
+    bool end=false;
+    
+bool getline()
 {
-    while (b.getc() != '$');
-    msg[0] = '$';  // wait for the start of a line
-    for (int i=1; i<256; i++) 
-    {
-        msg[i] = b.getc();
-        if (msg[i] == '\n') 
+    while (1)
+    {    
+        if( !b.readable() )
+        {
+            break;
+        }
+        char c = b.getc();
+        if (c == 36 ){start=true;end = false; i = 0;}
+        if (c == 10) {end=true; start = false;}
+        if (start)
         {
+            msg[i]=c;
+            i++;
+            if (i>255) break; //protect msg buffer!
+        }
+        if (end)
+        {
+            msg[i]=c;   
             msg[i+1] = 0;
-            return;
+            start = false;
+            end = true;
+            break;
         }
     }
-}
-
-int sample()
-{
-    while (1) 
-    {
-        getline();
-        return 1;
-    }
-}
-
-int samplepc()
-{
-    while (1) 
-    {
-        getline2();
-        return 1;
-    }
+    return end;
 }
 
 void keyPressedHeld( void )
@@ -1107,59 +1057,109 @@
 {
 }
 
-int getCheckSum(char *string) {
-int i;
-int XOR;
-int c;
+int getCheckSum(char *string)
+{
+    int i;
+    int XOR;
+    int c;
 // Calculate checksum ignoring any $'s in the string
-for (XOR = 0, i = 0; i < strlen(string); i++) {
-c = (unsigned char)string[i];
-if (c == '*') break;
-if (c != '$') XOR ^= c;
+    for (XOR = 0, i = 0; i < strlen(string); i++) 
+    {
+        c = (unsigned char)string[i];
+        if (c == '*') break;
+        if (c != '$') XOR ^= c;
+    }
+    return XOR;
 }
-return XOR;
+
+void Dispatch(char* line, bool config /* = false */)
+{
+    //pc.puts(line);
+    char* pointer;
+    char* Data[10]; //Can have max of 5 peices of data split by commas
+    int index = 0;
+    //Split data at commas
+    pointer = strtok(line, ",");
+    while(pointer != NULL) 
+    {
+        Data[index] = pointer;
+        pointer = strtok(NULL, ",");
+        index++;
+    }
+
+    if(strcmp(Data[0], "$BAUD") == 0) 
+    {
+       // pc.puts("\r\nWE GOT BAUD!!!\r\n");
+        gps_baud = atoi(Data[1]);
+        activate_antenna();
+    } 
+    else if(strcmp(Data[0], "$FGPSAUTO") == 0)
+    {
+
+    }
+    else if(strcmp(Data[0], "$GYRO") == 0)
+    {
+
+    }
+    else if(strcmp(Data[0], "$HEIGHT") == 0)
+    {
+
+    }
+}
+
+void Config_Startup()
+{
+    char line[256];
+    fp = fopen("/local/config.txt", "r");
+    while (fgets(line, sizeof(line), fp)) //Read through config file line by line
+    Dispatch(line, true); //Send line to dispatcher, true indicates its coming from the config file
+    fclose(fp);
+}
+
+void overide_config(int baud, int height, int x, int y, int z, char* freepilot_sets)
+{
+    fp = fopen("/local/config.txt", "w");
+    fprintf(fp, "\r\n$BAUD,%i\r\n$HEIGHT,%d\r\n$GYRO,%d,%d,%d\r\n%s",baud,height,x,y,z,freepilot_sets); //Rewrite text file for TESTING
+    fclose(fp);
 }
 
 int main()
 {
     bluetooth.baud(38400);
-    b.baud(38400);
-pc.baud(38400);
+    pc.baud(38400);
+    Config_Startup();
     motor_switch.setSampleFrequency(10000);
     motor_switch.attach_asserted_held( &keyPressedHeld );
     motor_switch.attach_deasserted_held( &keyReleasedHeld );
-initializeAccelerometer();
-calibrateAccelerometer();
- initializeGyroscope();
+    initializeAccelerometer();
+    calibrateAccelerometer();
+    initializeGyroscope();
    calibrateGyroscope();
-accelerometerTicker.attach(&sampleAccelerometer, 0.005);
-//debug_leds.atatch(&debugging_leds,2);
-gyroscopeTicker.attach(&sampleGyroscope, 0.005);
-filterTicker.attach(&filter, FILTER_RATE);
-angle_print.attach(&toprint,0.2);
-    activate_antenna();
+    accelerometerTicker.attach(&sampleAccelerometer, 0.005);
+    //debug_leds.atatch(&debugging_leds,2);
+    gyroscopeTicker.attach(&sampleGyroscope, 0.005);
+    filterTicker.attach(&filter, FILTER_RATE); //needing a timer here as gyro loop as filter loop need to be exactly at right timing
+    angle_print.attach(&toprint,0.2);
+
     while(1) 
     {
-        if ( antenna_active == 1 ) 
+        if ( antenna_active == 1 && b.readable()) 
         {
-            if ( b.readable()) 
+            if (getline()) 
             {
-                if (sample()) 
+                checksumm = getCheckSum(msg);
+                if ( pc.writeable()) 
                 {
-                    checksumm = getCheckSum(msg);
-                    if ( pc.writeable()) 
-                    {
-                        sprintf(output,"%s\r\n\0",msg);
-                        pc.puts(output);
-                    }
-                    gps_analyse(msg);
+                    sprintf(output,"%s\r\n\0",msg);
+                    pc.puts(output);
                 }
+                gps_analyse(msg);
             }
         }
 
         if ( bluetooth.readable()) 
         {
-            if (samplepc()) 
+            if (getline2()) 
             {
                 trim(msg2,"\r");
                 trim(msg2,"\n");