Diff: main.cpp

Revision:

34:c2bc9f9be7ff

Parent:

33:3e71c418e90d

Child:

35:f9caeb8ca31e

--- a/main.cpp	Mon Feb 02 18:24:03 2015 +0000
+++ b/main.cpp	Fri Feb 13 17:22:53 2015 +0000
@@ -8,15 +8,19 @@
 #include "Config.h"
 #include "imu_functions.h"
 #include "atoh.h"
+#include "checksum.h"
+#include <string.h>
+//#include "autosteer.h"
+//#include "conversion.h"
 
 #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
-
+ 
 char *version="FreePilot V2.11 Jtan 20, 2015\r\n";
 long lastsend_version=0;
 Timer vTimer; //this timer is int based! Max is 30 minutes
-
+ 
 int checksumm;
 double distance_from_line;
 double cm_per_deg_lon;
@@ -29,35 +33,36 @@
 Ticker gyroscopeTicker;
 Ticker filterTicker;
 Ticker  angle_print;
-
+ 
 //Motor
 PinDetect  motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
 DigitalOut enable_motor(p7);
-
+ 
 PwmOut pwm1(p22);
 PwmOut pwm2(p21);
-
+ 
 //equipment switches
 PinDetect  boom1(p20,PullUp); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
 PinDetect  boom2(p19); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
 PinDetect  boom3(p18); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
 PinDetect  boom4(p17); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
-
+ 
 char boom18; //1 byte
 char lastboom18; //1 byte
 char boomstate[8]={'$','F','B','S',0,13,10,0 };
-
+ 
+ double filterg = 100;
 Point position;
 Point looked_ahead;
 Point line_start;
 Point line_end;
 Point tilt_compensated_position;
 Point yaw_compensated_position;
-
+ 
 extern int gyro_pos;
-
+ 
 double distance_to_line;
-
+ 
 //FreePilot variables
 int timer_enabled;
 double motorspeed;
@@ -71,17 +76,17 @@
 Timer motTimer; //this timer is int based! Max is 30 minutes
 Timer btTimer; //measure time for Bluetooth communication
 long lastgetBT=0;
-
+ 
 int msg2_changed = 1;
 char* buffer;
 double meter_lat = 0;
 double meter_lon = 0;
-
+ 
 char msg[256]; //GPS line buffer
 char msg2[256];//PC line buffer
 int printing;
 int num_of_gps_sats;
-
+ 
 double decimal_lon;
 float longitude;
 float latitude;
@@ -108,7 +113,7 @@
 double velocity; // speed in knot
 int connect_time = 10000; //variable to change the time that the serial output all the strings in order to verify if the command was right.
 int connecting = 0; //are we still in phase of connecting? based on the connect_time value.
-
+ 
 int angle_send = 0;
 int correct_rmc = 1;
 double m_lat = 0;
@@ -121,49 +126,49 @@
 int active_AB = 0;
 double compensation_vector;
 char output[256];
-
+ 
 double yaw;
 double pitch;
 double roll;
-
+ 
 double a_x;
 double a_y;
 double a_z;
 double w_x;
 double w_y;
 double w_z;
-
+ 
 int readings[3];
-
+ 
 double Freepilot_lat;
 double Freepilot_lon;
 double Freepilot_lat1;
 double Freepilot_lon1;
 double Freepilot_bearing;
-
+ 
 volatile bool newline_detected = false;
-
+ 
 Point point_add(Point a, Point b)
 {
     return Point(a.GetX() + b.GetX(), a.GetY() + b.GetY());
 }
-
+ 
 Point point_sub(Point a , Point b)
 {
     return Point(a.GetX() - b.GetX(),  a.GetY() - b.GetY());
 }
-
+ 
 double get_yaw()
 {
     double yaw_angle = imuFilter.getYaw() * -1 ; // *-1 to reverse polarity as gyro is up-side-down 
     return yaw_angle;
 }
-
+ 
 void update_motor()
 {
     
 }
-
+ 
 double get_roll()
 {
     double roll_angle = 0;
@@ -185,7 +190,7 @@
     }
     return roll_angle;
 }
-
+ 
 double get_pitch()
 {
     double pitch_angle = 0;
@@ -207,26 +212,26 @@
      }
     return pitch_angle;
 }
-
+ 
 double dist_Point_to_Line( Point P, Point line_start, Point line_end)
 {
      Point v = point_sub(line_end,line_start);
      Point w = point_sub(P,line_start);
-
+ 
      double c1 = dot(w,v);
      double c2 = dot(v,v);
      double b = c1 / c2;
-
+ 
      Point resulting(b * v.GetX(),b*v.GetY());
      Point Pb = point_add(line_start, resulting);
      return d(P, Pb);
 }
-
+ 
 double lat_to_deg(char *s, char north_south)
 {
     int deg, min, sec;
     double fsec, val;
-
+ 
     deg  = ( (s[0] - '0') * 10) + s[1] - '0';
     min  = ( (s[2] - '0') * 10) + s[3] - '0';
     sec  = ( ((s[5] - '0') * 1000) + ((s[6] - '0') * 100) + ((s[7] - '0') * 10) + (s[8] - '0'));
@@ -236,10 +241,9 @@
     {
         val *= -1.0;
     }
-    decimal_latitude = val;
     return val;
 }
-
+ 
 // isLeft(): test if a point is Left|On|Right of an infinite 2D line.
 //    Input:  three points P0, P1, and P2
 //    Return: >0 for P2 left of the line through P0 to P1
@@ -250,7 +254,7 @@
     return ( (P1.GetY() - P0.GetY()) * (P2.GetX() - P0.GetX())
            - (P2.GetY() - P0.GetY()) * (P1.GetX() - P0.GetX()));
 }
-
+ 
 double lon_to_deg(char *s, char east_west)
 {
     int deg, min, sec;
@@ -264,10 +268,9 @@
     {
         val *= -1.0;
     }
-    decimal_lon = val;
     return val;
 }
-
+ 
 void nmea_gga(char *s)
 {
     char *token;
@@ -279,7 +282,7 @@
     char *qual      = (char *)NULL;
     char *altitude  = (char *)NULL;
     char *sats      = (char *)NULL;
-
+ 
     token = strtok(s, ",");
     while (token) 
     {
@@ -310,7 +313,7 @@
         token = strtok((char *)NULL, ",");
         token_counter++;
     }
-
+ 
     if (latitude && longitude && altitude && sats) 
     {
         decimal_latitude = lat_to_deg(latitude,  lat_dir[0]);
@@ -323,7 +326,7 @@
         gps_satellite_quality = 0;
     }
 }
-
+ 
 //from farmerGPS code
 void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2)
 {
@@ -337,32 +340,157 @@
     ydist = sinr * distance;
     lat2 = lat1 + (ydist / (69.09 * -1609.344));
     lon2 = lon1 - (xdist / (69.09 * 1609.344 * cos(lat1/57.295779513)));
-    return;
+  //  return;
 }
-
+ 
 Point compensation;
-double compensation_angle;
-//antenna compensation in cm
-void tilt_compensate()
-{
-    roll = get_roll();
-    compensation_vector = antennaheight * sin(roll);
-    compensation.SetX(compensation_vector * cos(get_yaw() *  -1 - (3.14159265359 / 2)));// 57.295779513));
-    compensation.SetY(compensation_vector * sin(get_yaw() *  -1 - (3.14159265359 / 2)));//  57.295779513));
-}
-void pitch_compensate()
-{
-    pitch = get_pitch();
-    compensation_vector = antennaheight * sin(pitch);
-    compensation.SetX(compensation.GetX() + compensation_vector * cos(get_yaw() *  -1 - (3.14159265359 / 2)));// /57.295779513));
-    compensation.SetY(compensation.GetY() + compensation_vector * sin(get_yaw() *  -1 - (3.14159265359 / 2)));// / 57.295779513));
-}
-
+ 
 void yaw_compensate()
 {
     yaw = get_yaw();
 }
+ 
+ float mX[2];
+float mY[2];
+float mZ[2];
+long _speedArr[10];
+long _delSpeed[2];
+//==================
 
+//Filters===========
+float BA;
+float BB;
+float AA;
+//==================
+double _dist = 0;
+double m_PI = 3.14159265359;
+double _lookA;
+double j_scale;
+double j_fGain;
+
+bool _acq = false;
+
+int autoGuide_A(double _gSpeed, double _distErr, int _acq,double j_phaseAdv, double j_tCenter, double lookA, double _scale, double _j_fGain)
+{
+    float T1T2r = (1 + sin(j_phaseAdv * m_PI / 180)) / (1 - sin(j_phaseAdv * m_PI / 180));
+    float T1 = sqrt(T1T2r) * j_tCenter;
+    float T2 = T1 / T1T2r;
+
+    float K = (1 + 2 * T1 / 0.2);
+    float L = (1 - 2 * T1 / 0.2);
+    float M = (1 + 2 * T2 / 0.2);
+    float N = (1 - 2 * T2 / 0.2);
+
+    BA = K / M;
+    BB = L / M;
+    AA = N / M;
+
+    _lookA = lookA;
+    j_scale = _scale;
+    j_fGain = _j_fGain;
+    
+    //int send = 0;
+    mX[1] = mX[0];
+    mX[0] = _distErr * j_fGain;
+    mY[1] = mY[0];
+    mY[0] = -AA * mY[1] + BA * mX[0] + BB * mX[1];
+    float y = mY[0] * j_scale;
+    y = y * (1 + _gSpeed / 100);
+    y = 127 + y;
+    if(_acq && y > 117 && y < 137) 
+    {
+        if(_distErr > 0 ) 
+        {
+            y += 10;
+        } 
+        else 
+        {
+            y -= 10;
+        }
+    }
+
+    if(y > 255) 
+    {
+        y = 255;
+    }
+    if(y < 0) 
+    {
+        y = 0;
+    }
+    return y;//j_guidance = y;
+}
+
+/*int LookAhead( double _gSpeed, double _distErr, int _LR, double _headErr)
+{
+    double temp = _distErr;
+    temp = abs(temp);
+    _headErr = 90-_headErr;
+   // double _headErr2 = 90 - _headErr;
+    //=========================================
+
+        double _abDist = _distErr;
+         _acq = false;
+         temp = abs(_abDist);
+        if(temp > 0.2) 
+        {
+            _acq = true;
+        }
+    if(_acq) 
+    {
+        if(_abDist < 0.15 && _abDist > -0.15)// && _headErr2 > 89.2) 
+        {
+            _acq = false;
+        }
+    }
+    //set_mode(_headErr,_distErr);
+    double ld = 0; //xvalue of look ahead point
+    double dist = 0;
+    double val = 0;
+    dist = _gSpeed  * _lookA; //Speed * look ahead time.
+    val = (_headErr / 180) * m_PI;
+    val = cos(val);
+    ld = dist * val;
+    if(_LR == 0) 
+    {
+        _distErr = _distErr - ld;
+    } 
+    else if(_LR == 1) 
+    {
+        _distErr = _distErr + ld;
+    }
+    return autoGuide_A(_gSpeed,_distErr,_acq);
+}
+*/
+
+
+
+void SetFilter(double j_phaseAdv, double j_tCenter, double lookA, double _scale, double _j_fGain)
+{
+    float T1T2r = (1 + sin(j_phaseAdv * m_PI / 180)) / (1 - sin(j_phaseAdv * m_PI / 180));
+    float T1 = sqrt(T1T2r) * j_tCenter;
+    float T2 = T1 / T1T2r;
+
+    float K = (1 + 2 * T1 / 0.2);
+    float L = (1 - 2 * T1 / 0.2);
+    float M = (1 + 2 * T2 / 0.2);
+    float N = (1 - 2 * T2 / 0.2);
+
+    BA = K / M;
+    BB = L / M;
+    AA = N / M;
+
+    _lookA = lookA;
+    j_scale = _scale;
+    j_fGain = _j_fGain;
+}
+
+ void pitch_and_roll(double real_bearing)
+{
+    pitch = get_pitch();
+    roll = get_roll();
+    compensation.SetX(antennaheight * tan(roll) * sin(real_bearing/ 57.295779513)-antennaheight * tan(pitch) * cos(real_bearing/ 57.295779513));
+    compensation.SetY(antennaheight * tan(roll) * cos(real_bearing/ 57.295779513)-antennaheight * tan(pitch) * sin(real_bearing/ 57.295779513));
+}
 
 void process_GPSHEIGHT(char* height_string)
 {
@@ -372,7 +500,7 @@
     token = strtok(height_string, ",");
     while (token) 
     {
-
+ 
         switch (token_counter) 
         {
             case 1:
@@ -388,9 +516,42 @@
         Config_Save();
     }
 }
+ 
 
+char* To_DMS(double dec_deg)
+{
+    char dms[128];
+    dec_deg = abs(dec_deg);
+    int d = (int)(dec_deg);
+    sprintf(dms,"%0.2i\0",d);
+    double m = (double)(((double)dec_deg - (double)d) * 60.0);
+    if (m < 10 ) {
+        sprintf(dms,"%s0%0.9f\0",dms,m);
+    } else {
+        sprintf(dms,"%s%0.9f\0",dms,m);
+    }
+    return dms;
+}
+
+char* To_DMS_lon(double dec_deg)
+{
+    char dms[128];
+    dec_deg = abs(dec_deg);
+    int d = (int)(dec_deg);
+    sprintf(dms,"%0.3i\0",d);
+    double m = (double)(((double)dec_deg - (double)d) * 60.0);
+    if (m < 10 ) {
+        sprintf(dms,"%s0%0.9f\0",dms,m);
+    } else {
+        sprintf(dms,"%s%0.9f\0",dms,m);
+    }
+    return dms;
+}
+
+ char rmc_cpy[256];
 void nmea_rmc(char *s)
 {
+   // strcpy(rmc_cpy, s);
     char *token;
     int  token_counter = 0;
     char *time   = (char *)NULL;
@@ -404,45 +565,43 @@
     char *longitude = (char *)NULL;
     char *lat_dir   = (char *)NULL;
     char *lon_dir   = (char *)NULL;
-
+    char magdd = 'E';
+ 
     token = strtok(s, ",*");
     while (token) 
     {
-        switch (token_counter) 
+             switch (token_counter) 
         {
-            case 9:
-                date   = token;
-                break;
             case 1:
                 time   = token;
                 break;
             case 2:
                 stat   = token;
                 break;
+            case 3:
+                latitude  = token;
+                break;
+            case 4:
+                lat_dir   = token;
+                break;
+            case 5:
+                longitude = token;
+                break;
+            case 6:
+                lon_dir   = token;
+                break;
             case 7:
                 vel    = token;
                 break;
             case 8:
                 trk    = token;
                 break;
+            case 9:
+                date   = token;
+                break;
             case 10:
                 magv   = token;
                 break;
-            case 11:
-                magd   = token;
-                break;
-            case 3:
-                latitude  = token;
-                break;
-            case 5:
-                longitude = token;
-                break;
-            case 4:
-                lat_dir   = token;
-                break;
-            case 6:
-                lon_dir   = token;
-                break;
         }
         token = strtok((char *)NULL, ",");
         token_counter++;
@@ -461,45 +620,101 @@
         speed_m_s = speed_km * 3600.0 / 1000.0;
         track      = atof(trk);
         magvar     = atof(magv);
-        magvar_dir = magd[0];
+       // magvar_dir = magd[0];
     }
-        decimal_latitude = lat_to_deg(latitude,  lat_dir[0]);
-        decimal_lon = lon_to_deg(longitude, lon_dir[0]);
-        position.SetX(decimal_latitude);
-        position.SetY(decimal_lon);
+       position.SetX(lat_to_deg(latitude,  lat_dir[0]));
+        position.SetY(lon_to_deg(longitude, lon_dir[0]));
         cm_per_deg_lat = 11054000;
         cm_per_deg_lon = 11132000 * cos(decimal_latitude);
         
-        // tilt_compensate(); //in centimeters
-        // pitch_compensate();
-        // yaw_compensate();
+        pitch_and_roll((track-90)*-1);// as to be real ebaring
         
         compensation.SetY(compensation.GetY() / cm_per_deg_lon);
         compensation.SetX(compensation.GetX() / cm_per_deg_lat);
+        
         position = point_add(position,compensation);
-
+ 
         double lookaheaddistance = lookaheadtime * speed_m_s;
-        get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,track,look_ahead_lon,look_ahead_lat);
+        get_latlon_byangle(position.GetX(),position.GetY(),lookaheaddistance,(track-90)*-1,look_ahead_lon,look_ahead_lat);
         looked_ahead.SetX(look_ahead_lat);
         looked_ahead.SetY(look_ahead_lon);
-        double filtering = 111111 / 2.0 + 111111 * cos(decimal_latitude)/2.0;
+        double filtering = sqrt(111111.0*111111.0 + 111111.0 * cos(decimal_latitude/57.295779513)*111111.0 * cos(decimal_latitude/57.295779513));
         distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end);
         double sign = isLeft( line_start, line_end, looked_ahead);
-        if ( sign < 0 )
-        {
-            distance_to_line =  distance_to_line; 
-        }
-        else if ( sign > 0 )
-        {
-            distance_to_line = -distance_to_line; 
-        }
+if ( sign < 0 )
+{
+    distance_to_line =  distance_to_line;
+    sign = 0;
+} else if ( sign > 0 )
+{
+    distance_to_line = -distance_to_line;
+    sign = 1;
+}
+ 
+   double ErrAngle = asin(abs(distance_to_line * filtering)/(sqrt(lookaheaddistance*lookaheaddistance + abs(distance_to_line * filtering*distance_to_line * filtering))))*57.295779513;
+   
+   //= acos( 1.0/((0.5 * abs(distance_to_line)/(lookaheaddistance * filtering) )))*57.295779513;
+       // int LookAhead(double _gSpeed, double _distErr, int _LR, double _headErr);
+             //  SetFilter(phaseadv, tcenter, lookaheadtime, scale,filterg);
+      // int motor_val = LookAhead( speed_m_s, abs(distance_to_line), sign, ErrAngle);
+       int motor_val = autoGuide_A(speed_m_s, abs(distance_to_line),false,phaseadv, tcenter, lookaheadtime, scale,filterg);
+    //    pc.printf("$MOTORSPEED,%i   ErrAngle:%f\r\n",motor_val,ErrAngle);
+        // sprintf(rebuilt_rmc,"$GPRMC,%s,%s,%f,%s,%f,%s,%s,%s,%s,%s,%s*\r\n",time,stat,position.GetX(),lat_dir,position.GetY(), lon_dir,vel,trk,date,magv,magd);
+    string rmc = "";
+    if(sizeof(time) > 0) {
+        rmc = "$GPRMC,";
+        rmc +=  string(time) + ",";
+    } else {
+        rmc = "$GPRMC,,";
+    }
+    if( sizeof(stat)>0 ) {
+        rmc +=(string(stat) + ",");
+    } else {
+        rmc += ",";
+    }
+    if ( sizeof(lat_dir) > 0 ) {
+        rmc +=( string(To_DMS(position.GetX())) + "," + string(lat_dir) +",");
 
-        //modify_rmc();
-        
-        sprintf(output,"$DIST_TO_LINE: % .12f   %f\r\n\0",distance_to_line * filtering, sign);
-        pc.puts(output);
+    } else {
+        rmc += ",,";
+    }
+    if ( sizeof(lon_dir) > 0 ) {
+        rmc += (string(To_DMS_lon(position.GetY())) + "," + string(lon_dir) + ",");
+    } else {
+        rmc += ",";
+        rmc += ",";
+    }
+    if (sizeof(vel) > 0 ) {
+        rmc += string(vel) + ",";
+    } else {
+        rmc += ",";
+    }
+    if ( sizeof(trk) > 0 ) {
+        rmc += string(trk) + ",";
+    } else {
+        rmc += ",";
+    }
+    if (sizeof(date) > 0) {
+        rmc += string(date) + ",";
+    } else {
+        rmc += ",";
+    }
+    if (sizeof(magv) > 0) {
+        rmc += string(magv) + ",";
+    } else {
+        rmc += ",";
+    }
+
+    char test[256];
+    sprintf(test,"%sW*",rmc);
+    sprintf(output,"%sW*%02X\r\n",rmc,getCheckSum(test));
+
+    bluetooth.puts(output);
+//pc.puts(output);
+    sprintf(output,"$DIST_TO_LINE: % .12f   %i   %f\r\n",distance_to_line * filtering,motor_val,ErrAngle);
+    pc.puts(output);
 }
-
+  
 void process_FGPSAB(char* ab)
 {
     char *token;
@@ -543,19 +758,20 @@
     line_end.SetX(Freepilot_lat1);
     line_end.SetY(Freepilot_lon1);
     active_AB = 1;
-
+ 
     sprintf(output, "$ABLINE:%f , %f, %f, %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY());
     pc.puts(output);
 }
-
+ 
 void autosteer_done()
 {
     //kill the autosteer once the timeout reech
     enable_motor = 0;
 }
-
+ 
 void process_FGPSAUTO(char* FGPSAUTO)
 {
+    //pc.puts(FGPSAUTO);
     char *token;
     int  token_counter = 0;
     char *ahead  = (char *)NULL;
@@ -565,6 +781,8 @@
     char *avg  = (char *)NULL;
     char *_kp = (char *)NULL;
     char *_ki  = (char *)NULL;
+        char *fg  = (char *)NULL;
+
     char *_kd = (char *)NULL;
     token = strtok(FGPSAUTO, ",");
     while (token) 
@@ -577,6 +795,9 @@
             case 2:
                 center = token;
                 break;
+            case 3:
+                fg = token;
+                break;
             case 4:
                 scl = token;
                 break;
@@ -612,11 +833,13 @@
         phaseadv = atof(phase);
         avgpos = atof(avg);
         tcenter = atof(center);
-        sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime);
-        pc.puts(output);
+        filterg = atof(fg);
+    //   / sprintf(output, "$SETTINGS:%f\r\n", lookaheadtime);
+       // pc.puts(output);
+        SetFilter(phaseadv, tcenter, lookaheadtime, scale,filterg);
     }
 }
-
+ 
 //sets pwm1 and pwm2 and enable_motor
 void process_ASTEER(char* asteer)
 {
@@ -662,22 +885,23 @@
             pwm2_speed = 0;
             enable_motor = 0;
         }
-        if(Authenticated)
-        {
+      //  if(Authenticated)
+       // {
            pwm1 = pwm1_speed;
            pwm2 = pwm2_speed;
-        }
-        else
-        {
-            sprintf(output,"$NOT AUTHENTICATED TO STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);     
-            pc.puts(output);
-            bluetooth.puts(output);
-        }
+      //  }
+        //else
+        //{
+         //   sprintf(output,"$NOT AUTHENTICATED TO STEER %f %d %f %f",motorspeed,enable_time,pwm1_speed,pwm2_speed);     
+         //   pc.puts(output);
+         //   bluetooth.puts(output);
+       // }
     }
 }
             
 void pc_analyse(char* pc_string)
 {
+   // pc.puts(pc_string);
     if (!strncmp(pc_string, "$ASTEER", 7)) 
     {
         process_ASTEER(pc_string);
@@ -694,6 +918,13 @@
         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);
+        bluetooth.puts(output);
+        Config_Save();
+    } 
     else if (!strncmp(pc_string, "$FGPS,",6)) 
     {
        int i=5;
@@ -707,7 +938,7 @@
           pc.putc(c);   
        }
     } 
-
+ 
     else if (!strncmp(pc_string, "$GPSHEIGHT",10)) 
     {
         process_GPSHEIGHT(pc_string);
@@ -715,13 +946,7 @@
         bluetooth.puts(output);
         Config_Save();
     } 
-    else if (!strncmp(pc_string, "$FGPSAUTO",9)) 
-    {
-        process_FGPSAUTO(pc_string);
-        sprintf(output,"%s\r\n",pc_string);
-        bluetooth.puts(output);
-        Config_Save();
-    } 
+
     else if (!strncmp(pc_string, "$FGPSAB",7)) 
     {
         process_FGPSAB(pc_string);
@@ -734,7 +959,7 @@
     }
     else if (!strncmp(pc_string, "$POSITION",9)) 
     {
-
+ 
         char* pointer;
         char* Data[5];
         int index = 0;
@@ -757,21 +982,22 @@
     {
     }
 }
-
+ 
 void gps_analyse(char* gps_string)
 {
-    pc.puts(gps_string);
+   // pc.puts(gps_string);
     //bluetooth.puts(gps_string);
     if (!strncmp(gps_string, "$GPRMC", 6)) 
     {
+     //   pc.puts(gps_string);
         nmea_rmc(gps_string); //analyse and decompose the rmc string
     }
 }
-
+ 
 int i2 = 0;
 bool end2 = false;
 bool start2 = false;
-
+ 
 bool getline2()
 {
     int gotstring=false;
@@ -812,8 +1038,8 @@
     }
     return gotstring;
 }
-
-
+ 
+ 
 int i=0;
 bool start=false;
 bool end=false;
@@ -855,38 +1081,39 @@
     }
     return end;
 }
-
+ 
 void keyPressedHeld( void )
 {
     motor_enable_state = "$ENABLE,1\r\n";
     motor_enable = 1;
     ledGREEN=1; //show green for being ready to steer
 }
-
+ 
 void keyReleasedHeld( void )
 {
-    motor_enable_state = "$ENABLE,0\r\n";
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+    motor_enable_state = "$ENABLE,1\r\n";
     motor_enable = 0;
     ledGREEN=0; 
 }
-
+ 
 void boom1PressedHeld( void )
 {
    // ledGREEN=1;
  boom18=boom18 & 0xFE;
 }
-
+ 
 void boom1ReleasedHeld( void )
 {
   //ledGREEN=0;  
   boom18=boom18 | 0x01;
 }
-
+ 
 void boom2PressedHeld( void )
 {  
    boom18=boom18 & 0xFD;
 }
-
+ 
 void boom2ReleasedHeld( void )
 {
    boom18=boom18 | 0x02;
@@ -895,68 +1122,27 @@
 {
      boom18=boom18 & 0xFB;
 }
-
+ 
 void boom3ReleasedHeld( void )
 {
   boom18=boom18 | 0x04;
 }
-
+ 
 void boom4PressedHeld( void )
 {
     boom18=boom18 & 0xF7;
 }
-
+ 
 void boom4ReleasedHeld( void )
 {
     boom18=boom18 | 0x08;
 }
-
+ 
 void toprint()
 {
     angle_send = 1;
 }
-
-char* checksum2;
-int getCheckSum(char *string)
-{
-    int i;
-    int XOR;
-    int c;
-    bool started = false;
-    for (XOR = 0, i = 0; i < strlen(string); i++) 
-    {
-        c = (unsigned char)string[i];
-        if ( c == '$' )started = true;
-        
-        if ( started == true )
-        {
-        if (c == '*')
-        {
-            break;
-        }
-        if (c != '$') XOR ^= c;
-        }
-    }
-    return XOR;
-}
-
-bool validate_checksum(char* validating)
-{
-    bool valid = false;
-    int tempo = getCheckSum(msg);
-    string token, mystring(msg);
-    while(token != mystring) 
-    {
-        token = mystring.substr(0,mystring.find_first_of("*"));
-        mystring = mystring.substr(mystring.find_first_of("*") + 1,2);
-    }
-    checksumm = atoh <uint16_t>(token.c_str());
-    if (tempo == checksumm) 
-    {
-        valid = true;
-    }
-    return valid;
-}
+ 
                 
 int main()
 {
@@ -964,14 +1150,14 @@
     bluetooth.baud(115200);
     gps.baud(38400);
     pc.baud(38400);
-
+ 
 //JH prepare and send version info
     vTimer.start();
     vTimer.reset();
     motTimer.start();
     motTimer.reset();
     lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
-    motor_enable_state = "$ENABLE,0\r\n";
+    motor_enable_state = "$ENABLE,1\r\n";
          initializeAccelerometer();
     calibrateAccelerometer();
     initializeGyroscope();
@@ -1012,7 +1198,7 @@
     motor_switch.setSampleFrequency(10000);
     motor_switch.attach_asserted_held( &keyPressedHeld );
     motor_switch.attach_deasserted_held( &keyReleasedHeld );
-
+ 
     accelerometerTicker.attach(&sampleAccelerometer, 0.005);
     gyroscopeTicker.attach(&sampleGyroscope, 0.005);
     filterTicker.attach(&filter, FILTER_RATE);
@@ -1032,10 +1218,14 @@
         
         if ( antenna_active == 1 && gps.readable()) 
         {
-            if (getline(true)) 
+            if (getline(false)) 
             {
                 if ( validate_checksum(msg)) 
                 {
+                    if ( bluetooth.writeable())
+                    {
+                 //   bluetooth.puts(msg);
+                    }
                     gps_analyse(msg);
                 } 
                 else 
@@ -1050,10 +1240,9 @@
            {
                btTimer.reset();
                lastgetBT=  btTimer.read_ms();
-               x++;
-                trim(msg2," ");
-                sprintf(output,"%d %s",x,msg2);
-                pc.puts(output);
+             //  x++;
+              //  trim(msg2," ");
+             //   pc.puts(msg2);
                 pc_analyse(msg2);
             }
         }
@@ -1082,7 +1271,7 @@
         if ( print_euler == 1 && angle_send == 1 ) //&& reading == 0) 
         {
             sprintf(output,"$EULER,%f,%f,%f\r\n",toDegrees(get_roll()),toDegrees(get_pitch()),toDegrees(get_yaw()));
-            pc.puts(output);
+           // pc.puts(output);
             bluetooth.puts(output);
             angle_send = 0;
         }