Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 
72  uint8_t req_idx = (f->idx - f->delay + DELAYED_FIRST_ORDER_LOWPASS_FILTER_BUFFER_SIZE) %
74  return f->buffer[req_idx];
75 }
76 
85 {
86  /* alpha = 1 / ( 1 + omega_c * Ts) */
87  f->omega = omega;
88  f->alpha = (f->sample_frequency << DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT) /
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
uint16_t
unsigned short uint16_t
Definition: types.h:16
DELAYED_FIRST_ORDER_LOWPASS_FILTER_BUFFER_SIZE
#define DELAYED_FIRST_ORDER_LOWPASS_FILTER_BUFFER_SIZE
Definition: delayed_first_order_lowpass_filter.h:32
delayed_first_order_lowpass_filter_t
Definition: delayed_first_order_lowpass_filter.h:35
delayed_first_order_lowpass_initialize
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
Definition: delayed_first_order_lowpass_filter.h:120
uint32_t
unsigned long uint32_t
Definition: types.h:18
paparazzi.h
delayed_first_order_lowpass_filter_t::sample_frequency
uint16_t sample_frequency
Definition: delayed_first_order_lowpass_filter.h:36
DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT
#define DELAYED_FIRST_ORDER_LOWPASS_FILTER_FILTER_ALPHA_SHIFT
Definition: delayed_first_order_lowpass_filter.h:33
delayed_first_order_lowpass_filter_t::alpha
int32_t alpha
Definition: delayed_first_order_lowpass_filter.h:39
delayed_first_order_lowpass_set_omega
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
Definition: delayed_first_order_lowpass_filter.h:84
delayed_first_order_lowpass_filter_t::max_inc
uint16_t max_inc
Definition: delayed_first_order_lowpass_filter.h:40
uint8_t
unsigned char uint8_t
Definition: types.h:14
delayed_first_order_lowpass_set_delay
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
Definition: delayed_first_order_lowpass_filter.h:101
delay
#define delay(ms)
Definition: cc2500_compat.h:171
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
int32_t
signed long int32_t
Definition: types.h:19
delayed_first_order_lowpass_propagate
static int32_t delayed_first_order_lowpass_propagate(struct delayed_first_order_lowpass_filter_t *f, int32_t input)
delayed_first_order_lowpass_propagate
Definition: delayed_first_order_lowpass_filter.h:54
delayed_first_order_lowpass_filter_t::idx
uint8_t idx
Definition: delayed_first_order_lowpass_filter.h:42
delayed_first_order_lowpass_filter_t::delay
uint8_t delay
Definition: delayed_first_order_lowpass_filter.h:38
delayed_first_order_lowpass_filter_t::omega
uint32_t omega
Definition: delayed_first_order_lowpass_filter.h:37
delayed_first_order_lowpass_filter_t::buffer
int32_t buffer[DELAYED_FIRST_ORDER_LOWPASS_FILTER_BUFFER_SIZE]
Definition: delayed_first_order_lowpass_filter.h:41