low_pass_zoh_filter.h

Go to the documentation of this file.

/*
 * Copyright (C) 2025 Justin Dubois <j.p.g.dubois@student.tudelft.nl>
 *
 * 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, see
 * <http://www.gnu.org/licenses/>.
 */
#ifndef LOW_PASS_ZOH_FILTER_H
#define LOW_PASS_ZOH_FILTER_H
 
#include "std.h"
#include "math/pprz_algebra_int.h"
 
 
struct FirstOrderZOHLowPass {
  float discrete_time_constant;
  float last_in;
  float last_out;
};
 
static inline void init_first_order_zoh_low_pass(struct FirstOrderZOHLowPass *filter, const float tau,
    const float sample_time,
    float value)
{
  filter->discrete_time_constant = exp(-sample_time / tau);
  filter->last_in = value;
  filter->last_out = value;
}
 
static inline float update_first_order_zoh_low_pass(struct FirstOrderZOHLowPass *filter, const float value)
{
  float out = (1.0f - filter->discrete_time_constant) * value + filter->discrete_time_constant * filter->last_out;
  filter->last_in = value;
  filter->last_out = out;
  return out;
}
 
static inline float reset_first_order_zoh_low_pass(struct FirstOrderZOHLowPass *filter, const float value)
{
  filter->last_in = value;
  filter->last_out = value;
  return value;
}
 
static inline float get_first_order_zoh_low_pass(const struct FirstOrderZOHLowPass *filter)
{
  return filter->last_out;
}
 
static inline void update_first_order_zoh_low_pass_tau(struct FirstOrderZOHLowPass *filter, const float tau,
    const float sample_time)
{
  filter->discrete_time_constant = exp(-sample_time / tau);
}
 
#endif // LOW_PASS_ZOH_FILTER_H