filter.c

Go to the documentation of this file.

/**************************************************************************/
#include "filter.h"
#include "system.h"
#include "foc.h"
#include "config.h"
 
volatile Filter_Structure_t velocity_filter;
 
void filter_coefficient_config(Filter_Structure_t *param, float cutoff_freq, float sample_freq, float coefficient) {
    /* clear the value of the first order low pass filter parameter handler */
    user_memset(param, 0x00, sizeof(Filter_Structure_t));
 
    /* the RC time constant is calculated from the cut-off frequency */
    float time_const = 1.0f / (6.2831852f * cutoff_freq);
 
    /* calculation of digital low-pass filter coefficients */
    param->coefficient1 = (1.0f / (1.0f + time_const * sample_freq)) * coefficient;
    param->coefficient2 = (time_const * sample_freq) / (1.0f + time_const * sample_freq);
    param->current_result = 0;
    param->last_result = 0;
}
 
void filter_config(void) {
    filter_coefficient_config((Filter_Structure_t *) &velocity_filter,
                              TIM13_FREQUENCY, SPEED_UP_FREQ, SPEED_COEFFICIENT);
}
 
float filter_update_value(Filter_Structure_t *param, short value) {
    /* judge whether the input parameters are abnormal,
     * calculate the output value of the first-order low-pass filter */
    if (value < 200 && value > -200)
        param->current_result = param->coefficient1 * (float) value + param->coefficient2 * param->last_result;
 
    /* update the output value of the previous time */
    param->last_result = param->current_result;
    return param->current_result;
}