changed to 50hz to reduce cpu util

Author: Johannes Formann

src/autosteer.cpp

@@ -45,8 +45,8 @@ AutoPID pid(
 
 constexpr time_t Timeout = 2000;
 
-void autosteerWorker100Hz( void* z ) {
-  constexpr TickType_t xFrequency = 10;
+void autosteerWorker( void* z ) {
+  constexpr TickType_t xFrequency = 20;
   TickType_t xLastWakeTime = xTaskGetTickCount();
 
   pid.setTimeStep( xFrequency );
@@ -80,7 +80,7 @@ void autosteerWorker100Hz( void* z ) {
 
         static uint8_t loopCounter = 0;
 
-        if ( ++loopCounter >= 10 ) {
+        if ( ++loopCounter >= 5 ) {
           loopCounter = 0;
 
           //close enough to center, 4 cm, remove any correction
@@ -235,7 +235,7 @@ if ( driveValue < 0 ) {
     {
       static uint8_t loopCounter = 0;
 
-      if ( ++loopCounter >= 10 ) {
+      if ( ++loopCounter >= 5 ) {
         loopCounter = 0;
 
         if ( initialisation.outputType != SteerConfig::OutputType::None ) {
@@ -654,5 +654,5 @@ void initAutosteer() {
 
   }
 
-  xTaskCreate( autosteerWorker100Hz, "autosteerWorker", 4096, NULL, 3, NULL );
+  xTaskCreate( autosteerWorker, "autosteerWorker", 4096, NULL, 3, NULL );
 }

src/sensor.cpp

@@ -98,18 +98,18 @@ class  FilterBuLp2_3 {
       v[1] = 0.0;
     }
   private:
-    float v[3];
-  public:
-    float step( float x ) { //class II
-      v[0] = v[1];
-      v[1] = v[2];
-      v[2] = ( 2.008336556421122521e-2 * x )
-             + ( -0.64135153805756306422 * v[0] )
-             + ( 1.56101807580071816339 * v[1] );
-      return
-        ( v[0] + v[2] )
-        + 2 * v[1];
-    }
+  		float v[3];
+  	public:
+  		float step(float x) { //class II
+  			v[0] = v[1];
+  			v[1] = v[2];
+  			v[2] = (6.745527388907189559e-2 * x)
+  				 + (-0.41280159809618854894 * v[0])
+  				 + (1.14298050253990091107 * v[1]);
+  			return
+  				 (v[0] + v[2])
+  				+2 * v[1];
+  		}
 } wheelAngleSensorFilter;
 
 void genericImuCalibrationCalcMagnetometer() {
@@ -451,7 +451,7 @@ void updateImuData(float gx, float gy, float gz, float ax, float ay, float az, f
 
 void sensorWorkerSteeringPoller( void* z ) {
   vTaskDelay( 2000 );
-  constexpr TickType_t xFrequency = 10;
+  constexpr TickType_t xFrequency = 20;
   TickType_t xLastWakeTime = xTaskGetTickCount();
 
   for ( ;; ) {
@@ -504,6 +504,8 @@ void sensorWorkerSteeringPoller( void* z ) {
 
         steerSetpoints.wheelAngleRaw = wheelAngleTmp;
 
+        // now zero position and counts per degre of the sensor is nown (raw value)
+
         if ( steerConfig.wheelAngleSensorType == SteerConfig::WheelAngleSensorType::TieRodDisplacement ) {
           if ( steerConfig.wheelAngleFirstArmLenght != 0 && steerConfig.wheelAngleSecondArmLenght != 0 &&
                steerConfig.wheelAngleTrackArmLenght != 0 && steerConfig.wheelAngleTieRodStroke != 0 ) {