Diff: main.cpp

Revision:

40:a68cc1a1a1e7

Parent:

39:6767d4c840f9

Child:

41:a26acd346c2f

--- a/main.cpp	Thu Mar 05 16:26:52 2015 +0000
+++ b/main.cpp	Fri Mar 06 16:31:19 2015 +0000
@@ -573,19 +573,21 @@
     } else if ( forward_sign == 1 ) {
         error = error;
     }
-    if ( position_dist < 0.50 ) {
+  //if ( abs(position_dist) < 0.5 ) {
         if ( forward_sign == position_sign ) {
-            if ( position_dist > (dist_line + (0.5 * position_dist))) { // && abs(position_dist) <
-                error = error * (dist_line * filter_g - position_dist * phase_adv);
+            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)*0.5 - position_dist * phase_adv) * 0.5;//.8 in order to come back less on line than we came on it
+            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 * (20 - heading_err) * 5;
-    }
+    //} else { //target coming back at 15-20 degrees on the line
+        error = error;// + heading_err  * 2 ;
+    //}
 
     error = error * scale;
     if ( error > 0 ) {
@@ -693,7 +695,6 @@
         }
         token_counter++;
     }
-
     if (stat!= '\0' && date!= '\0' && time!= '\0') {
         hour       = (char)((time[0] - '0') * 10) + (time[1] - '0');
         minute     = (char)((time[2] - '0') * 10) + (time[3] - '0');
@@ -707,9 +708,7 @@
         speed_m_s = speed_km * 3600.0 / 1000.0;
         track      = atof(trk);
         magvar     = atof(magv);
-        // magvar_dir = magd[0];
     }
-
     double angle = ((int)((track-90 )* -1 + 360) + 180) % 360 - 180;
     double diff_angle = Freepilot_bearing - angle;
     diff_angle = ((int)diff_angle + 180) % 360 - 180;
@@ -855,27 +854,6 @@
     line_start.SetY(Freepilot_lon);
     line_end.SetX(Freepilot_lat1);
     line_end.SetY(Freepilot_lon1);
-    /*
-        double diff_angle = Freepilot_bearing - track;
-        diff_angle = ((int)diff_angle + 180) % 360 - 180;
-
-        if ( abs(diff_angle) > 90 ) {
-            line_end.SetX(Freepilot_lat);
-            line_end.SetY(Freepilot_lon);
-            line_start.SetX(Freepilot_lat1);
-            line_start.SetY(Freepilot_lon1);
-            Freepilot_bearing = Freepilot_bearing + 180;
-            Freepilot_bearing = ((int) Freepilot_bearing + 360) % 360 - 180;
-        } else {
-            line_start.SetX(Freepilot_lat);
-            line_start.SetY(Freepilot_lon);
-            line_end.SetX(Freepilot_lat1);
-            line_end.SetY(Freepilot_lon1);
-            Freepilot_bearing = Freepilot_bearing;
-        }
-    */
-    pc.puts(bearing.c_str());
-    pc.puts("\r\n");
 
     active_AB = 1;
 
@@ -957,7 +935,12 @@
         _ID = Config_GetID();
         Config_Save();
     } else if (!strncmp(pc_string, "$GPSBAUD",8)) {
-        process_GPSBAUD(pc_string);
+           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());
         Config_Save();
         sprintf(output,"%s %d\r\n",pc_string,gps_baud);
         //   pc.puts(output);