3 files changed, 41 insertions, 1 deletions
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index bee00c049..68bd680a8 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -523,7 +523,7 @@ uint32_t MillisecLoop(uint32_t tick)
     }
     //ROM_IntMasterEnable();
 
-    //loop_Run_Read_Speed(); - just for testing
+
     return OK;
 }
 /******************************************************************************
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
index f37759943..72774461a 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -36,4 +36,40 @@ float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_P
 	return output;
 }
 
+float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
+{
+    float error;
+    float derivative;
+    float output;
+
+    //Calculate P,I,D
+    error = _setPoint - _mesuredParam;
+
+    //In case of error too small then stop integration
+    if(abs(error) > params->epsilon)
+    {
+        *_integral = *_integral + error*params->dt;
+    }
+
+    derivative = (error - *_pre_error)/params->dt;
+    float IntegralErrorMultiplier;
+    float ProportionalErrorMultiplier;
+
+    output = params->Kp*error/params->ProportionalErrorMultiplier + params->Ki**_integral/params->IntegralErrorMultiplier + params->Kd*derivative;
+
+    //Saturation Filter
+    if(output > params->MAX)
+    {
+        output = params->MAX;
+    }
+    else if(output < params->MIN)
+    {
+        output = params->MIN;
+    }
+
+    //Update error
+    *_pre_error = error;
+
+    return output;
+}
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
index 03c363131..4792be9ec 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
@@ -12,7 +12,11 @@ typedef struct
     float Kp;
     float Kd;
     float Ki;
+    float IntegralErrorMultiplier;
+    float ProportionalErrorMultiplier;
+
 }PID_Config_Params;
 float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral);
+float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral);
 
 #endif /* MODULES_PIDALGO_H_ */