Commit 5f3833ec authored by Stefan's avatar Stefan
Browse files

filter korrigiert

parent 0cb04197
......@@ -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
*/
......
#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;
......
......@@ -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.
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment