Diff: main.cpp

Revision:

38:b5352d6f8166

Parent:

37:ac60a8a0ae8a

Child:

39:6767d4c840f9

--- a/main.cpp	Sun Feb 22 21:55:45 2015 +0000
+++ b/main.cpp	Wed Mar 04 13:11:33 2015 +0000
@@ -11,6 +11,7 @@
 #include "checksum.h"
 #include <string.h>
 
+//buttun filter
 
 #define dot(u,v)   ((u).GetX() * (v).GetX()+ (u).GetY() * (v).GetY())
 #define norm(v)     sqrt(dot(v,v))     // norm = length of  vector
@@ -33,6 +34,8 @@
 Ticker filterTicker;
 Ticker  angle_print;
 
+//Ticker PID_TICK;
+
 //Motor
 PinDetect  motor_switch(p16); //pinDetect is close to digitalIn, althought, it can detect holds and detect the debounce.
 DigitalOut enable_motor(p7);
@@ -168,6 +171,126 @@
 {
 
 }
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//PID
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+double outMin_ = 0.0;
+double outMax_ = 510.0;
+double inMin_ = 0.0;
+double inMax_ = 10.0;
+double prevControllerOutput_ = 5.0;
+double accError_ = 0.0;
+double prevProcessVariable_ = 5.0;
+double pParam_;
+double iParam_;
+double dParam_;
+double tSample_ = 0.2;
+double Kc_ ;
+double tauR_ ;
+double tauD_ ;
+bool inAuto = true;
+double bias_ = 0.0;
+bool usingFeedForward = true;
+double processVariable_ = 5.0;
+
+void setTunings(float Kc, float tauI, float tauD)
+{
+    //Verify that the tunings make sense.
+    if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
+        return;
+    }
+    //Store raw values to hand back to user on request.
+    pParam_ = Kc;
+    iParam_ = tauI;
+    dParam_ = tauD;
+    float tempTauR;
+    if (tauI == 0.0) {
+        tempTauR = 0.0;
+    } else {
+        tempTauR = (1.0 / tauI) * tSample_;
+    }
+    //For "bumpless transfer" we need to rescale the accumulated error.
+    if (inAuto) {
+        if (tempTauR == 0.0) {
+            accError_ = 0.0;
+        } else {
+            accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
+        }
+    }
+    Kc_   = Kc;
+    tauR_ = tempTauR;
+    tauD_ = tauD / tSample_;
+
+}
+
+void reset(void)
+{
+    double     outSpan_ = outMax_ - outMin_;
+    double     inSpan_ = inMax_ - inMin_;
+    float scaledBias = 0.0;
+
+    //  if (usingFeedForward) {
+    scaledBias = (bias_ - outMin_) / outSpan_;
+
+    prevControllerOutput_ = scaledBias;
+    prevProcessVariable_  = (processVariable_ - inMin_) / inSpan_;
+
+    //Clear any error in the integral.
+    accError_ = 0;
+}
+
+float compute(double setPoint_)
+{
+    double     outSpan_ = outMax_ - outMin_;
+    double     inSpan_ = inMax_ - inMin_;
+    //Pull in the input and setpoint, and scale them into percent span.
+    float scaledPV = (processVariable_ - inMin_) / inSpan_;
+
+    if (scaledPV > 1.0) {
+        scaledPV = 1.0;
+    } else if (scaledPV < 0.0) {
+        scaledPV = 0.0;
+    }
+
+    float scaledSP = (setPoint_ - inMin_) / inSpan_;
+    if (scaledSP > 1.0) {
+        scaledSP = 1;
+    } else if (scaledSP < 0.0) {
+        scaledSP = 0;
+    }
+    float error = scaledSP - scaledPV;
+
+    //Check and see if the output is pegged at a limit and only
+    //integrate if it is not. This is to prevent reset-windup.
+    if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
+        accError_ += error;
+    }
+
+    //Compute the current slope of the input signal.
+    float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
+    float scaledBias = 0.0;
+
+    if (usingFeedForward) {
+        scaledBias = (bias_ - outMin_) / outSpan_;
+    }
+
+    //Perform the PID calculation.
+    double controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
+    //Make sure the computed output is within output constraints.
+    if (controllerOutput_ < 0.0) {
+        controllerOutput_ = 0.0;
+    } else if (controllerOutput_ > 1.0) {
+        controllerOutput_ = 1.0;
+    }
+
+    //Remember this output for the windup check next time.
+    prevControllerOutput_ = controllerOutput_;
+    //Remember the input for the derivative calculation next time.
+    prevProcessVariable_  = scaledPV;
+
+    //Scale the output from percent span back out to a real world number.
+    return ((controllerOutput_ * outSpan_) + outMin_);
+}
 
 double get_roll()
 {
@@ -210,6 +333,7 @@
 
     Point resulting(b * v.GetX(),b*v.GetY());
     Point Pb = point_add(line_start, resulting);
+
     return d(P, Pb);
 }
 
@@ -229,6 +353,7 @@
     return val;
 }
 
+int dir = 1;
 // 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
@@ -242,6 +367,14 @@
     } else {
         isleft = -1;
     }
+    
+    if ( dir == 1 )
+    {
+        isleft = isleft;
+        } else
+        {
+            isleft *= -1;
+            }
     return (int)isleft;
 }
 
@@ -254,6 +387,7 @@
     sec  = ( ((s[6] - '0') * 1000) + ((s[7] - '0') * 100) + ((s[8] - '0') * 10) + (s[9] - '0'));
     fsec = (double)((double)sec /10000.0);
     val  = (double)deg + ((double)((double)min/60.0)) + (fsec/60.0);
+
     if (east_west == 'W') {
         val *= -1.0;
     }
@@ -328,7 +462,6 @@
     ydist = sinr * distance;
     lat2 = lat1 + (ydist / (69.09 * -1609.344));
     lon2 = lon1 - (xdist / (69.09 * 1609.344 * cos(lat1/57.295779513)));
-    //  return;
 }
 
 Point compensation;
@@ -353,7 +486,6 @@
     char *height  = (char *)NULL;
     token = strtok(height_string, ",");
     while (token) {
-
         switch (token_counter) {
             case 1:
                 height = token;
@@ -375,7 +507,6 @@
     int d = (int)(dec_deg);
     sprintf(dms,"%0.2i\0",d);
     double m = (double)(((double)dec_deg - (double)d) * 60.0);
-    // m = m + d*100;
     if (m < 10 ) {
         sprintf(dms,"%s0%0.9f\0",dms,m);
     } else {
@@ -425,7 +556,6 @@
         time_till_stop = atoi(asteer_time);
         //autosteer_timeout.attach_us(autosteer_done,(double)enable_time *  (double)1000.0);
         if ( motor_enable == 0 ) {
-
         } else {
             if ( motorspeed > 127.0 ) {
                 pwm2_speed = 0.0;
@@ -434,7 +564,6 @@
             } else if ( motorspeed < 127.0 ) {
                 pwm1_speed = 0.0;
                 pwm2_speed = ( ((double)127-(double)motorspeed) / 128.0 );
-
             } else {
                 pwm1_speed = 0;
                 pwm2_speed = 0;
@@ -445,63 +574,102 @@
             pwm1 = pwm1_speed;
             pwm2 = pwm2_speed;
             enable_motor = 1;
-            pc.printf("SPEED,%f,%f\r\n",pwm1_speed,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);
-            // }
+            //  pc.puts("SPEED,%f,%f\r\n",pwm1_speed,pwm2_speed);
+            //}
+
         }
     }
 }
 
+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
 {
-    double error = 0;
-    scale = scale - 5;
+    dist_line = abs(dist_line);
+    double error = 1;
+   // scale = scale - 5;
     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);
 
-    error = 1;
-
     if ( forward_sign == -1 ) {
         error = error * -1;
     } else if ( forward_sign == 1 ) {
         error = error;
     }
+
     if ( forward_sign == position_sign ) {
-        if ( position_dist > (dist_line + 0.2)) {
+        // double temp = dist_line;
+
+        /*  if ( lastval > abs(dist_line)) {
+              dist_line = dist_line / 2.0;
+          }
+          lastval = temp;
+        */
+        if ( position_dist > (dist_line + (0.2 * position_dist))) { // && abs(position_dist) <
+            //added * position_dist
             error = error * (dist_line * filter_g - position_dist * phase_adv);
         } else {
             error = error * dist_line * filter_g;
         }
     } else {
-        error = error * (dist_line * filter_g - position_dist * phase_adv) * 0.8;
+        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
     }
+
     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 rmc_cpy[256];
+char *strsep(char **stringp, char *delim)
+{
+    char *s;
+    const char *spanp;
+    int c, sc;
+    char *tok;
+    if ((s = *stringp) == NULL)
+        return (NULL);
+    for (tok = s;;) {
+        c = *s++;
+        spanp = delim;
+        do {
+            if ((sc = *spanp++) == c) {
+                if (c == 0)
+                    s = NULL;
+                else
+                    s[-1] = 0;
+                *stringp = s;
+                return (tok);
+            }
+        } while (sc != 0);
+    }
+    /* NOTREACHED */
+}
+
+Point old_position;
+
+//char rmc_cpy[256];
 void nmea_rmc(char *s)
 {
+    // pc.puts(s);
     // strcpy(rmc_cpy, s);
+    // sprintf(s,"%s\0",s);
     char *token;
     int  token_counter = 0;
     char *time   = (char *)NULL;
@@ -510,15 +678,15 @@
     char *vel    = (char *)NULL;
     char *trk    = (char *)NULL;
     char *magv   = (char *)NULL;
-    //  char *magd   = (char *)NULL;
+    char *magd   = (char *)NULL;
+    char *latit  = "";
+    char *longit = "";
     char *latitude  = (char *)NULL;
     char *longitude = (char *)NULL;
     char *lat_dir   = (char *)NULL;
     char *lon_dir   = (char *)NULL;
-    // char magdd = 'E';
 
-    token = strtok(s, ",*");
-    while (token) {
+    while ((token = strsep(&s, ",")) != NULL) {
         switch (token_counter) {
             case 1:
                 time   = token;
@@ -527,12 +695,17 @@
                 stat   = token;
                 break;
             case 3:
-                latitude  = token;
+                if ( token ) 
+                {
+                    latit  = token;
+                    latitude  = token;
+                }
                 break;
             case 4:
                 lat_dir   = token;
                 break;
             case 5:
+                longit = token;
                 longitude = token;
                 break;
             case 6:
@@ -550,11 +723,14 @@
             case 10:
                 magv   = token;
                 break;
+            case 11:
+                magd = token;
+                break;
         }
-        token = strtok((char *)NULL, ",");
         token_counter++;
     }
-    if (stat && date && time) {
+
+    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');
         second     = (char)((time[4] - '0') * 10) + (time[5] - '0');
@@ -569,99 +745,121 @@
         magvar     = atof(magv);
         // magvar_dir = magd[0];
     }
-    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);
+    if ( longit != '\0' && latit != '\0' ) {
+        old_position = position;
+
 
-    pitch_and_roll((track-90)*-1);// as to be real bearing
+        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);
 
-    compensation.SetY(compensation.GetY() / cm_per_deg_lon);
-    compensation.SetX(compensation.GetX() / cm_per_deg_lat);
+//        pitch_and_roll((track-90)*-1);// as to be real bearing
 
-    position = point_add(position,compensation);
+        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-90)*-1,look_ahead_lon,look_ahead_lat);
-    looked_ahead.SetX(look_ahead_lat);
-    looked_ahead.SetY(look_ahead_lon);
-    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 lookaheaddistance = lookaheadtime * speed_m_s;
+        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 = 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) * isLeft( line_start,line_end,looked_ahead) * filtering;
 
-    double ErrAngle = asin(abs(distance_to_line * filtering)/(sqrt(lookaheaddistance*lookaheaddistance + abs(distance_to_line * filtering*distance_to_line * filtering))))*57.295779513;
+        int val = freepilot(position, looked_ahead, 0.0, distance_to_line, scale,  filterg , phaseadv,tcenter ,filtering);
 
-    int val = freepilot(position, looked_ahead, ErrAngle, abs(distance_to_line*filtering), scale,  filterg,phaseadv,tcenter,filtering);//dist in meters
+        char command[128];
 
-    char command[128];
-    sprintf(command,"$ASTEER,%i,%i\r\n",val,500);
-    pc.puts(command);
-    process_ASTEER(command);
-
+        sprintf(command,"$ASTEER,%i,%i\r\n",val,250);    //(int)((((val/2)-127)/scale)+127),500);
+        pc.puts(command);
+        process_ASTEER(command);
+    }
     string rmc = "";
-    if(sizeof(time) > 0) {
+    if(time!= '\0') {
         rmc = "$GPRMC,";
         rmc +=  string(time) + ",";
     } else {
         rmc = "$GPRMC,,";
     }
-    if( sizeof(stat)>0 ) {
+    if(stat!= '\0') {
         rmc +=(string(stat) + ",");
     } else {
         rmc += ",";
     }
-    if ( sizeof(lat_dir) > 0 ) {
+    if ( latit != '\0' && lat_dir!= '\0') {
         rmc +=( string(To_DMS(position.GetX())) + "," + string(lat_dir) +",");
-
     } else {
         rmc += ",,";
     }
-    if ( sizeof(lon_dir) > 0 ) {
+    if ( longit != '\0' && lon_dir!= '\0' ) {
         rmc += (string(To_DMS_lon(position.GetY())) + "," + string(lon_dir) + ",");
     } else {
         rmc += ",";
         rmc += ",";
     }
-    if (sizeof(vel) > 0 ) {
-        rmc += string(vel) + ",";
+    if (vel!= '\0') {
+        rmc += (string(vel) + ",");
     } else {
         rmc += ",";
     }
-    if ( sizeof(trk) > 0 ) {
+    if ((trk)!= '\0') {
         rmc += string(trk) + ",";
     } else {
         rmc += ",";
     }
-    if (sizeof(date) > 0) {
+    if (date!= '\0') {
         rmc += string(date) + ",";
     } else {
         rmc += ",";
     }
-    if (sizeof(magv) > 0) {
+    if (magv!= '\0') {
         rmc += string(magv) + ",";
     } else {
         rmc += ",";
     }
+    if (magd!= '\0') {
+        rmc += string(magd) + ",W";
+    } else {
+        rmc += ",W";
+    }
 
     char test[256];
-    sprintf(test,"%sW*",rmc);
-    sprintf(output,"%sW*%02X\r\n",rmc,getCheckSum(test));
-
+    sprintf(test,"%s*\0",rmc.c_str());
+    sprintf(output,"%s*%02X\r\n\0",rmc.c_str(),getCheckSum(test));
+    
     bluetooth.puts(output);
-//pc.puts(output);
-    sprintf(output,"$DIST_TO_LINE: % .12f\r\n",distance_to_line * filtering);//,motor_val,ErrAngle);
-    pc.puts(output);
 }
 
+void line_dir(Point current_pos, Point old_pos, Point start, Point end, double line_heading, Point &start2, Point &end2)
+{
+    double ahead_lat = 0.0;
+    double ahead_lon = 0.0;
+//void get_latlon_byangle(double lat1, double lon1, double distance,double angle, double &lon2, double &lat2)
+    get_latlon_byangle(start.GetX(), start.GetY(), (double)2500.0, ((line_heading - 90) * -1), ahead_lon, ahead_lat);
+    
+    if ( sqrt((ahead_lon - current_pos.GetY())*(ahead_lon - current_pos.GetY()) + (ahead_lat - current_pos.GetX())*(ahead_lat - current_pos.GetY()))
+            > sqrt((ahead_lon - old_pos.GetY())*(ahead_lon - old_pos.GetY()) + (ahead_lat - old_pos.GetX())*(ahead_lat - old_pos.GetX()))) {
+Point temp = line_start;
+line_start = line_end;
+line_end = temp;
+            
+        }
+    
+    }
+
+
 void process_FGPSAB(char* ab)
 {
-    //pc.puts(ab);
     char *token;
     int  token_counter = 0;
     char *line_lat  = (char *)NULL;
     char *line_lon  = (char *)NULL;
     char *line_lat1  = (char *)NULL;
     char *line_lon1  = (char *)NULL;
-    char *bearing  = (char *)NULL;
+    //char *bearing  = (char *)NULL;
+    string bearing = "";
     token = strtok(ab, ",");
     while (token) {
         switch (token_counter) {
@@ -678,7 +876,13 @@
                 line_lon1 = token;
                 break;
             case 5:
-                bearing = token;
+                for (int n=0; n < sizeof(token); n++) {
+                    if ( token[n] == '*' ) {
+                        break;
+                    } else {
+                        bearing += token[n];
+                    }
+                }
                 break;
         }
         token = strtok((char *)NULL, ",");
@@ -688,19 +892,24 @@
     Freepilot_lat = atof(line_lat);
     Freepilot_lon1  = atof(line_lon1);
     Freepilot_lat1 = atof(line_lat1);
-    Freepilot_bearing = atof(bearing);
+    Freepilot_bearing = atof(bearing.c_str());
+
     line_start.SetX(Freepilot_lat);
     line_start.SetY(Freepilot_lon);
     line_end.SetX(Freepilot_lat1);
     line_end.SetY(Freepilot_lon1);
+
+    pc.puts(bearing.c_str());
+    pc.puts("\r\n");
+    
     active_AB = 1;
 
-    sprintf(output, "$ABLINE:%f , %f, %f, %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY());
+   // line_dir(position ,old_position, line_start, line_end, (atof(bearing.c_str())- 90) * -1, line_start,line_end);
+
+    sprintf(output, "$ABLINE:%f , %f, %f, %f,  %f\r\n",line_start.GetX(),line_start.GetY(),line_end.GetX(),line_end.GetY(),Freepilot_bearing);
     pc.puts(output);
 }
 
-
-
 void process_FGPSAUTO(char* FGPSAUTO)
 {
     //pc.puts(FGPSAUTO);
@@ -714,8 +923,8 @@
     char *_kp = (char *)NULL;
     char *_ki  = (char *)NULL;
     char *fg  = (char *)NULL;
-
     char *_kd = (char *)NULL;
+    
     token = strtok(FGPSAUTO, ",");
     while (token) {
         switch (token_counter) {
@@ -754,19 +963,22 @@
         kp = atof(_kp);
         ki = atof(_ki);
         kd = atof(_kd);
+
+        // setTunings(kp , ki ,kd);
     }
     if ( phase && center && scl && avg && ahead ) {
         lookaheadtime = atof(ahead);
-        scale = atof(scl);
+        scale = atof(scl); ///////////////////////////////////////////////////////////////////////////////
         phaseadv = atof(phase);
         avgpos = atof(avg);
         tcenter = atof(center);
         filterg = atof(fg);
+
+            scale = 5 - scale;
+
     }
 }
 
-
-
 void pc_analyse(char* pc_string)
 {
     // pc.puts(pc_string);
@@ -779,7 +991,7 @@
         process_GPSBAUD(pc_string);
         Config_Save();
         sprintf(output,"%s %d\r\n",pc_string,gps_baud);
-        pc.puts(output);
+        //   pc.puts(output);
     } else if (!strncmp(pc_string, "$FGPSAUTO",9)) {
         process_FGPSAUTO(pc_string);
         sprintf(output,"%s\r\n",pc_string);
@@ -807,7 +1019,6 @@
         calibrateAccelerometer();
         Config_Save();
     } else if (!strncmp(pc_string, "$POSITION",9)) {
-
         char* pointer;
         char* Data[5];
         int index = 0;
@@ -820,9 +1031,8 @@
             pointer = strtok(NULL, ",");
             index++;
         }
-        //int temp_pos =
         gyro_pos = atoi(Data[1]);
-        pc.printf("POSITION=%i\r\n",gyro_pos);//("POSITION=");
+        //      pc.puts("POSITION=%i\r\n",gyro_pos);//("POSITION=");
         Config_Save();
     } else {
     }
@@ -830,10 +1040,10 @@
 
 void gps_analyse(char* gps_string)
 {
-    // pc.puts(gps_string);
-    //bluetooth.puts(gps_string);
+    //pc.puts(gps_string);
+    // bluetooth.puts(gps_string);
     if (!strncmp(gps_string, "$GPRMC", 6)) {
-        //   pc.puts(gps_string);
+        // pc.puts(gps_string);
         nmea_rmc(gps_string); //analyse and decompose the rmc string
     } else {
         bluetooth.puts(gps_string);
@@ -985,6 +1195,12 @@
     angle_send = 1;
 }
 
+double last_yaw = 0;
+int counter = 0;
+bool bear = false;
+double lastroll = 0;
+double lastpitch = 0;
+
 int main()
 {
     bluetooth.baud(115200);
@@ -994,6 +1210,7 @@
 //JH prepare and send version info
     vTimer.start();
     vTimer.reset();
+
     motTimer.start();
     motTimer.reset();
     lastsend_motorstate=motTimer.read_ms()-6500; //should trigger in 5s
@@ -1001,16 +1218,16 @@
 
     btTimer.start();
     btTimer.reset();
-    lastgetBT=  btTimer.read_ms();
+    lastgetBT= btTimer.read_ms();
 
-    pc.puts(version);
     bluetooth.puts(version);
+
     lastsend_version=vTimer.read_ms()-18000;
-
-    Config_Startup();
-    _ID = Config_GetID();
-    Config_Save();
-
+    //  pc.puts("test\r\n");
+    /* Config_Startup();
+     _ID = Config_GetID();
+     Config_Save();
+    */
     boom1.attach_asserted_held( &boom1PressedHeld );
     boom1.attach_deasserted_held( &boom1ReleasedHeld );
     boom1.setSampleFrequency(); //default = 20 ms
@@ -1047,6 +1264,9 @@
     angle_print.attach(&toprint,0.2);
     activate_antenna();
     autosteer_timeout.start();
+
+    setTunings(0.25, 5, 1);
+
     while(1) {
         //JH send version information every 10 seconds to keep Bluetooth alive
         if ((vTimer.read_ms()-lastsend_version)>25000) {
@@ -1063,6 +1283,7 @@
         if ( antenna_active == 1 && gps.readable()) {
             if (getline(false)) {
                 if ( validate_checksum(msg)) {
+                    //pc.puts(msg);
                     gps_analyse(msg);
                 } else {
                     pc.puts("INVALID!!!!\r\n");
@@ -1073,6 +1294,7 @@
             if (getline2()) {
                 btTimer.reset();
                 lastgetBT=  btTimer.read_ms();
+                //     pc.puts(msg2);
                 pc_analyse(msg2);
             }
         }
@@ -1083,7 +1305,7 @@
 
         if ( ((motTimer.read_ms()-lastsend_motorstate)>8000) || (motor_enable!=lastmotor_enable)) {
             bluetooth.puts(motor_enable_state);
-            pc.puts(motor_enable_state);
+            // pc.puts(motor_enable_state);
             motTimer.reset();
             lastsend_motorstate=motTimer.read_ms();
             lastmotor_enable=motor_enable;
@@ -1091,13 +1313,28 @@
         if (boom18!=lastboom18) {
             boomstate[4]=boom18 | 0x80; //
             bluetooth.puts(boomstate);
-            pc.puts(boomstate);
+            // pc.puts(boomstate);
             lastboom18=boom18;
         }
-        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()));
+        if ( print_euler == 1 && angle_send == 1 ) { //&& reading == 0){
+            lastpitch = 0.8*lastpitch + 0.2 * (toDegrees(get_pitch()*3.5));
+
+            lastroll = 0.8 * lastroll + 0.2 * toDegrees(get_roll()*3.5);
+
+            double dps = - last_yaw;
+            last_yaw =toDegrees( imuFilter.getYaw()) * -1;
+            dps = (dps + last_yaw) * 5; // update every 200 ms, so for dps need *5
+
+            sprintf(output,"$EULER,%f,%f,%f\r\n",lastroll,lastpitch,dps);
+            //      pc.puts(output);
             bluetooth.puts(output);
             angle_send = 0;
+
+            counter++;
+            if ( bear == false  && counter > 3 ) { //reinitialise the gyro after  600ms for the reference to be changed.
+                imuFilter.reset();
+                bear = true;
+            }
         }
     }
 }