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, const 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, const 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 reset_first_order_low_pass(struct FirstOrderLowPass *filter, const float value)
{
  filter->last_in = value;
  filter->last_out = value;
  return value;
}
 
static inline float get_first_order_low_pass(const struct FirstOrderLowPass *filter)
{
  return filter->last_out;
}
 
static inline void update_first_order_low_pass_tau(struct FirstOrderLowPass *filter, const float tau,
    const float sample_time)
{
  filter->time_const = 2.0f * tau / sample_time;
}
 
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, const float tau, const float Q,
    const float sample_time,
    float value)
{
  float K = tanf(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 reset_second_order_low_pass(struct SecondOrderLowPass *filter, const float value)
{
  filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value;
  return value;
}
 
static inline float update_second_order_low_pass(struct SecondOrderLowPass *filter, const 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(const 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, const float cut_off,
    const 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, const 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(const struct SecondOrderLowPass_int *filter)
{
  return filter->o[0];
}
 
typedef struct SecondOrderLowPass Butterworth2LowPass;
 
static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time,
    const 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, const float value)
{
  return update_second_order_low_pass((struct SecondOrderLowPass *)filter, value);
}
 
static inline float reset_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
{
  return reset_second_order_low_pass((struct SecondOrderLowPass *)filter, value);
}
 
static inline float get_butterworth_2_low_pass(const Butterworth2LowPass *filter)
{
  return get_second_order_low_pass((const struct SecondOrderLowPass *)filter);
}
 
typedef struct SecondOrderLowPass_int Butterworth2LowPass_int;
 
static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, const float cut_off,
    const 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, const 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(const 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, const float tau, const float sample_time,
    const 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, const 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(const Butterworth4LowPass *filter)
{
  return filter->lp2.o[0];
}
 
static inline void reset_butterworth_4_low_pass(Butterworth4LowPass *filter, const float value)
{
  filter->lp1.i[0] = filter->lp1.i[1] = filter->lp1.o[0] = filter->lp1.o[1] = filter->lp2.i[0] = filter->lp2.i[1] =
                                          filter->lp2.o[0] = filter->lp2.o[1] = value;
}
 
typedef struct {
  struct SecondOrderLowPass_int lp1;
  struct SecondOrderLowPass_int lp2;
} Butterworth4LowPass_int;
 
static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, const float cut_off,
    const 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, const 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(const Butterworth4LowPass_int *filter)
{
  return filter->lp2.o[0];
}
 
#endif