filter korrigiert

HalloRobot/libProject/include/filter.h

@@ -19,11 +19,6 @@
  */
 float complementary_filter(float phi_acc, float gyro_rate, uint16_t f_sampling);
 
-/*! \brief Implements a iir Filter
- * \param phi_acc angle phi calculated from accelero data
- * \returns filtered phi_acc
- */
-float iir_filter(float phi_acc);
 
 /*! \brief Resets filter angle
  */

HalloRobot/libProject/src/control.c

 #include "control.h"
 #include <math.h>
 #include "ezs_io.h"
-#include "filter.h"
 
 //defines
 #define OUT_MAX 100 //maximum output in percent
@@ -92,6 +91,24 @@ float pid_control(float e)
     return new_out;
 }
 
+static float iir_filter(float phi_acc)
+{
+    static float in_1 = 0;
+    static float in_2 = 0;
+    static float out_1 = 0;
+    static float out_2 = 0;
+    const float a[2] = {-1.9112,0.915};
+    const float b[3] = {9.4469e-4,0.0019,9.4469e-4};
+
+    float new_out = b[0]*phi_acc + b[1]*in_1 + b[2]*in_2- a[0]*out_1 - a[1]*out_2;
+
+    out_2 = out_1;
+    out_1 = new_out;
+    in_2 = in_1;
+    in_1 = phi_acc;    
+    return new_out;
+}
+
 /*
  * Implements Offset-controller.
  * Feeds back correction control_output to correct offset.
@@ -104,7 +121,9 @@ float offset_control(float motor_signal)
 
     // sum_offset += K*motor_signal/F_CONTROL;
 
-    float new_offset = iir_filter(K*motor_signal/F_CONTROL);
+    float new_offset = iir_filter(K*motor_signal);
+    // sum_offset = sum_offset + iir_filter(K*motor_signal); 
+    // float new_offset = sum_offset
     if(new_offset > max_offset){
         new_offset = max_offset;
         sum_offset -= motor_signal/F_CONTROL;

HalloRobot/libProject/src/filter.c

@@ -5,10 +5,7 @@ static float phi_cor = 0;
 
 /* IIR filter with butterworth coefficients */
 
-/*
- * Implements a IIR filter with butterworth coefficients
- */
-float iir_filter(float phi_acc)
+static float iir_filter(float phi_acc)
 {
     static float in_1 = 0;
     static float in_2 = 0;
@@ -26,6 +23,7 @@ float iir_filter(float phi_acc)
     return new_out;
 }
 
+
 /*
  * Implements a basic complementary filter,
  * representing a low-pass for accelero and high-pass for gyro.