Diff: main.cpp

Revision:

45:ecd8c2e27948

Parent:

44:e9d5cd98273d

Child:

46:d7d6dc429153

--- a/main.cpp	Mon Mar 09 17:36:36 2015 +0000
+++ b/main.cpp	Tue Mar 10 23:14:18 2015 +0000
@@ -81,6 +81,7 @@
 
 char msg[256]; //GPS line buffer
 char msg2[256];//PC line buffer
+char* result;
 int printing;
 int num_of_gps_sats;
 
@@ -434,58 +435,6 @@
     }
 }
 
-/*double lastval = 0;
-//gets the motor value between -255 and 255 (- means left, positive means right)
-//distance in meters, always positive
-//angle in degrees
-//Points in lat/lon format
-int freepilot(Point current, Point target, double heading_err, double dist_line, double scale, double filter_g, double phase_adv, double center, double filtering)//dist in meters
-{
-    dist_line = abs(dist_line);
-    double error = 1;
-    int position_sign = isLeft( line_start, line_end, current);
-    int forward_sign = isLeft(line_start,line_end, target);
-    double position_dist = abs(dist_Point_to_Line( current,line_start,line_end) * filtering);
-
-    if ( forward_sign == -1 ) {
-        error = error * -1;
-    } else if ( forward_sign == 1 ) {
-        error = error;
-    }
-    //if ( abs(position_dist) < 0.5 ) {
-    if ( forward_sign == position_sign ) {
-        if ( position_dist > dist_line ) { // && abs(position_dist) <
-            // error = error * (dist_line * filter_g - (position_dist * phase_adv));
-            // error = 0;
-        } else {
-            // error = error * (dist_line * filter_g - (position_dist * phase_adv)*0.8);
-            error = error * dist_line * filter_g;
-        }
-    } else { //
-        error = (error * ((dist_line * filter_g) - (position_dist * phase_adv)))*0.8;//.8 in order to come back less on line than we came on it
-    }
-    //} else { //target coming back at 15-20 degrees on the line
-    error = error;// + heading_err  * 2 ;
-    //}
-
-    error = error * scale;
-    if ( error > 0 ) {
-        error = error + center;
-    } else if (error < 0 ) {
-        error = error - center;
-    }
-    if ( error > 255 ) {
-        error = 255;
-    } else if ( error < -255 ) {
-        error = -255;
-    }
-
-    error = error + 255;
-    error = (int)(error / 2);
-
-    return (int)error;
-}
-*/
 char *strsep(char **stringp, char *delim)
 {
     char *s;
@@ -911,13 +860,14 @@
                 line_lon1 = token;
                 break;
             case 5:
-                for (int n=0; n < sizeof(token); n++) {
+             /*  for (int n=0; n < sizeof(token); n++) {
                     if ( token[n] == '*' ) {
                         break;
                     } else {
                         bearing += token[n];
                     }
-                }
+                }*/
+                bearing = token;
                 break;
         }
         token = strtok((char *)NULL, ",");
@@ -1015,15 +965,9 @@
         _ID = Config_GetID();
         // Config_Save();
     } else if (!strncmp(pc_string, "$GPSBAUD",8)) {
-        string mystring = pc_string;
-        string baud = pc_string;
-        if ( mystring.find('*') > 0 ) {
-            string baud = mystring.substr(0,mystring.find('*'));
-        }
-        process_GPSBAUD((char*)baud.c_str());
+
+        process_GPSBAUD(pc_string);
         // Config_Save();
-        sprintf(output,"%s %d\r\n",pc_string,gps_baud);
-        //   pc.puts(output);
     } else if (!strncmp(pc_string, "$FGPSAUTO",9)) {
         process_FGPSAUTO(pc_string);
         sprintf(output,"%s\r\n",pc_string);
@@ -1315,9 +1259,9 @@
         }
         if ( antenna_active == 1 && gps.readable()) {
             if (getline(false)) {
-                if ( validate_checksum(msg)) {
+                if ( validate_checksum(msg,result)) {
                     //pc.puts(msg);
-                    gps_analyse(msg);
+                    gps_analyse(result);
                 } else {
                     pc.puts("INVALID!!!!\r\n");
                 }
@@ -1325,10 +1269,12 @@
         }
         if ( bluetooth.readable()) {
             if (getline2()) {
-                btTimer.reset();
-                lastgetBT=  btTimer.read_ms();
-                //     pc.puts(msg2);
-                pc_analyse(msg2);
+                if ( validate_checksum(msg2,result)) {
+                    btTimer.reset();
+                    lastgetBT=  btTimer.read_ms();
+                    //     pc.puts(msg2);
+                    pc_analyse(result);
+                }
             }
         }
         if (  btTimer.read_ms()-lastgetBT>1000) {