low_pass_filter.h

Go to the documentation of this file.

/*
 * Copyright (C) 2013 Gautier Hattenberger
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */

#ifndef LOW_PASS_FILTER_H
#define LOW_PASS_FILTER_H

#include "std.h"
#include "math/pprz_algebra_int.h"

#define INT32_FILT_FRAC  8

struct FirstOrderLowPass {
  float time_const;
  float last_in;
  float last_out;
};

static inline void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time,
    float value)
{
  filter->last_in = value;
  filter->last_out = value;
  filter->time_const = 2.0f * tau / sample_time;
}

static inline float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
{
  float out = (value + filter->last_in + (filter->time_const - 1.0f) * filter->last_out) / (1.0f + filter->time_const);
  filter->last_in = value;
  filter->last_out = out;
  return out;
}

static inline float get_first_order_low_pass(struct FirstOrderLowPass *filter)
{
  return filter->last_out;
}

struct SecondOrderLowPass {
  float a[2]; 
  float b[2]; 
  float i[2]; 
  float o[2]; 
};


static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter, float tau, float Q, float sample_time,
    float value)
{
  float K = sample_time / (2.0f * tau);
  float poly = K * K + K / Q + 1.0f;
  filter->a[0] = 2.0f * (K * K - 1.0f) / poly;
  filter->a[1] = (K * K - K / Q + 1.0f) / poly;
  filter->b[0] = K * K / poly;
  filter->b[1] = 2.0f * filter->b[0];
  filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
}

static inline float update_second_order_low_pass(struct SecondOrderLowPass *filter, float value)
{
  float out = filter->b[0] * value
              + filter->b[1] * filter->i[0]
              + filter->b[0] * filter->i[1]
              - filter->a[0] * filter->o[0]
              - filter->a[1] * filter->o[1];
  filter->i[1] = filter->i[0];
  filter->i[0] = value;
  filter->o[1] = filter->o[0];
  filter->o[0] = out;
  return out;
}

static inline float get_second_order_low_pass(struct SecondOrderLowPass *filter)
{
  return filter->o[0];
}

struct SecondOrderLowPass_int {
  int32_t a[2]; 
  int32_t b[2]; 
  int32_t i[2]; 
  int32_t o[2]; 
  int32_t loop_gain; 
};

static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, float cut_off, float Q,
    float sample_time, int32_t value)
{
  struct SecondOrderLowPass filter_temp;
  float tau = 7.0f / (44.0f * cut_off);
  float K = sample_time / (2.0f * tau);
  float poly = K * K + K / Q + 1.0f;
  float loop_gain_f;

  filter_temp.a[0] = 2.0f * (K * K - 1.0f) / poly;
  filter_temp.a[1] = (K * K - K / Q + 1.0f) / poly;
  filter_temp.b[0] = K * K / poly;
  filter_temp.b[1] = 2.0f * filter_temp.b[0];
  loop_gain_f = 1.0f / filter_temp.b[0];

  filter->a[0] = BFP_OF_REAL((filter_temp.a[0] * loop_gain_f), INT32_FILT_FRAC);
  filter->a[1] = BFP_OF_REAL((filter_temp.a[1] * loop_gain_f), INT32_FILT_FRAC);
  filter->b[0] = BFP_OF_REAL(1, INT32_FILT_FRAC);
  filter->b[1] = 2 * filter->b[0];
  filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
  filter->loop_gain = BFP_OF_REAL(loop_gain_f, INT32_FILT_FRAC);
}

static inline int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, int32_t value)
{
  int32_t out = filter->b[0] * value
                + filter->b[1] * filter->i[0]
                + filter->b[0] * filter->i[1]
                - filter->a[0] * filter->o[0]
                - filter->a[1] * filter->o[1];

  filter->i[1] = filter->i[0];
  filter->i[0] = value;
  filter->o[1] = filter->o[0];
  filter->o[0] = out / (filter->loop_gain);
  return filter->o[0];
}

static inline int32_t get_second_order_low_pass_int(struct SecondOrderLowPass_int *filter)
{
  return filter->o[0];
}

typedef struct SecondOrderLowPass Butterworth2LowPass;

static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
{
  init_second_order_low_pass((struct SecondOrderLowPass *)filter, tau, 0.7071, sample_time, value);
}

static inline float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
{
  return update_second_order_low_pass((struct SecondOrderLowPass *)filter, value);
}

static inline float get_butterworth_2_low_pass(Butterworth2LowPass *filter)
{
  return filter->o[0];
}

typedef struct SecondOrderLowPass_int Butterworth2LowPass_int;

static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time,
    int32_t value)
{
  init_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, cut_off, 0.7071, sample_time, value);
}

static inline int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, int32_t value)
{
  return update_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, value);
}

static inline int32_t get_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter)
{
  return filter->o[0];
}

typedef struct {
  struct SecondOrderLowPass lp1;
  struct SecondOrderLowPass lp2;
} Butterworth4LowPass;

static inline void init_butterworth_4_low_pass(Butterworth4LowPass *filter, float tau, float sample_time, float value)
{
  init_second_order_low_pass(&filter->lp1, tau, 1.30651, sample_time, value);
  init_second_order_low_pass(&filter->lp2, tau, 0.51184, sample_time, value);
}

static inline float update_butterworth_4_low_pass(Butterworth4LowPass *filter, float value)
{
  float tmp = update_second_order_low_pass(&filter->lp1, value);
  return update_second_order_low_pass(&filter->lp2, tmp);
}

static inline float get_butterworth_4_low_pass(Butterworth4LowPass *filter)
{
  return filter->lp2.o[0];
}

typedef struct {
  struct SecondOrderLowPass_int lp1;
  struct SecondOrderLowPass_int lp2;
} Butterworth4LowPass_int;

static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, float cut_off, float sample_time,
    int32_t value)
{
  init_second_order_low_pass_int(&filter->lp1, cut_off, 1.30651, sample_time, value);
  init_second_order_low_pass_int(&filter->lp2, cut_off, 0.51184, sample_time, value);
}

static inline int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, int32_t value)
{
  int32_t tmp = update_second_order_low_pass_int(&filter->lp1, value);
  return update_second_order_low_pass_int(&filter->lp2, tmp);
}

static inline int32_t get_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter)
{
  return filter->lp2.o[0];
}

#endif