Diff: autosteer.h

Revision:

56:456d454d9ced

Parent:

54:2405c62c1049

Child:

57:0299098b2d0e

--- a/autosteer.h	Fri Mar 27 22:44:29 2015 +0000
+++ b/autosteer.h	Sun Mar 29 16:03:18 2015 +0000
@@ -2,6 +2,13 @@
 #define PI 3.14159265359
 #endif
 
+#ifndef AUTOSTEER_H
+#define AUTOSTEER_H
+
+#include <string.h>
+
+using namespace std;
+
 double m_Tcenter[5] = {0,0,0,0,0};
 double mphaseadv[5]= {0,0,0,0,0};
 double morder[5]= {0,0,0,0,0};
@@ -80,7 +87,7 @@
 }
 //double d = 0;
 
-string Steer(int ActiveTime,int value)
+std::string Steer(int ActiveTime,int value)
 {
     string sRet = "";
     if (value < 0) value = 0;
@@ -93,7 +100,7 @@
     return (sRet);
 }
 
-string ControlSteerFilter(int ActiveTime,  double distAUTOL, double speed, double FilterGain, double min, double max,double SCALE)
+std::string ControlSteerFilter(int ActiveTime,  double distAUTOL, double speed, double FilterGain, double min, double max,double SCALE)
 {
     string sRet = "";
     int N = 3;
@@ -118,13 +125,18 @@
             mz[N - 1] = mz[N];
         }
     }
+    SCALE = SCALE / 10;
     double y1 = (double)mz[N]; // y1 used to preserve value of mz[N];        // mz is now the output
     //modify scale depending on distance!
     double mscale = 0.1 * abs(distAUTOL) + SCALE;
+    if ( mscale > 5* SCALE )
+    {
+        mscale = 5*SCALE;
+    }
     y = (double)(y1 * mscale);      // scale it here   if neccesary
     y = (double)(y * pow((10.0 / speed), SpeedN)); //SpeedN varies the gain factor with speed  n=0 to 1 or more. n=0 should give  y=
 
-    y = y * mscale;
+  //  y = y * mscale;
 
     if ( y < -max ) {
         y = -max;
@@ -146,4 +158,5 @@
     }
 
     return (sRet);
-}
\ No newline at end of file
+}
+#endif
\ No newline at end of file