Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
delayed_first_order_lowpass_filter.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Bart Slinger
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 #ifndef DELAYED_FIRST_ORDER_LOWPASS_FILTER_H
28 #define DELAYED_FIRST_ORDER_LOWPASS_FILTER_H
29 
30 #include "paparazzi.h"
31 
32 #define DELAYED_FIRST_ORDER_LOWPASS_FILTER_BUFFER_SIZE 20
33 #define DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT 14
34 
43 };
44 
55  int32_t input)
56 {
57  int32_t prev = f->buffer[f->idx];
59  f->idx = next_idx;
60  f->buffer[next_idx] = (f->alpha * prev + ((1 << DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT) - f->alpha) *
62 
63  /* Check if new value exceeds maximum increase */
64  if ((f->buffer[next_idx] - prev) > f->max_inc) {
65  f->buffer[next_idx] = prev + f->max_inc;
66  }
67  /* Also negative case */
68  if ((f->buffer[next_idx] - prev) < -f->max_inc) {
69  f->buffer[next_idx] = prev - f->max_inc;
70  }
71 
74  return f->buffer[req_idx];
75 }
76 
85 {
86  /* alpha = 1 / ( 1 + omega_c * Ts) */
87  f->omega = omega;
89  (f->sample_frequency + f->omega);
90 }
91 
102 {
103  /* Delay cannot be more than buffer size minus one */
106  } else {
107  f->delay = delay;
108  }
109 }
110 
121  uint32_t omega, uint8_t delay, uint16_t max_inc, uint16_t sample_frequency)
122 {
123  /* Set sample frequency */
124  f->sample_frequency = sample_frequency;
125  /* Set delay */
127 
128  /* Set omega and calculate alpha */
130 
131  /* Set maximum increase per cycle */
132  f->max_inc = max_inc;
133 
134  /* Clear the buffer */
135  f->idx = 0;
137  f->buffer[i] = 0;
138  }
139 }
140 #endif
static void delayed_first_order_lowpass_initialize(struct delayed_first_order_lowpass_filter_t *f, uint32_t omega, uint8_t delay, uint16_t max_inc, uint16_t sample_frequency)
delayed_first_order_lowpass_initialize
unsigned short uint16_t
Definition: types.h:16
static int32_t delayed_first_order_lowpass_propagate(struct delayed_first_order_lowpass_filter_t *f, int32_t input)
delayed_first_order_lowpass_propagate
#define DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT
static void delayed_first_order_lowpass_set_omega(struct delayed_first_order_lowpass_filter_t *f, uint32_t omega)
delayed_first_order_lowpass_set_omega
unsigned long uint32_t
Definition: types.h:18
signed long int32_t
Definition: types.h:19
unsigned char uint8_t
Definition: types.h:14
int32_t buffer[DELAYED_FIRST_ORDER_LOWPASS_FILTER_BUFFER_SIZE]
#define DELAYED_FIRST_ORDER_LOWPASS_FILTER_BUFFER_SIZE
static void delayed_first_order_lowpass_set_delay(struct delayed_first_order_lowpass_filter_t *f, uint8_t delay)
delayed_first_order_lowpass_set_delay