Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[tests] Test filter stability for low filter frequency (#3399)

Browse Source
This commit is contained in:
Ewoud Smeur
2024-11-05 09:56:13 +01:00
committed by GitHub
parent 67edd8ab68
commit 90ac935509
2 changed files with 92 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/test/Makefile
+3
View File
@@ -29,6 +29,9 @@ test_geo: test_geo_conversions.c ../math/pprz_trig_int.c ../math/pprz_algebra_in
test_alloc: test_alloc.c ../math/wls/wls_alloc.c ../math/qr_solve/r8lib_min.c ../math/qr_solve/qr_solve.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) -DWLS_N_U=8 -DWLS_N_V=4
test_filter: test_filter.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_tt: test_tilt_twist.c ../math/pprz_algebra_float.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
sw/airborne/test/test_filter.c
+89
View File
@@ -0,0 +1,89 @@
/*
* Copyright (C) 2024 Ewoud Smeur <e.j.j.smeur@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, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file test_filter.c
*
* Test stability of Butterworth 2nd order low pass filter
*/
#include <stdio.h>
#include "std.h"
#include "filters/low_pass_filter.h"
Butterworth2LowPass testfilter;
void print_filter_coef(Butterworth2LowPass *filter);
void test_butterworth(float cutoff_hz, float periodic_frequency, int16_t num_samples);
int main(int argc, char **argv)
{
float cutoff_hz = 0.1f; //hz
float periodic_frequency = 500; //hz
int16_t num_samples = 15000;
test_butterworth(cutoff_hz, periodic_frequency, num_samples);
print_filter_coef(&testfilter);
}
/*
* function to test filter stability
*/
void test_butterworth(float cutoff_hz, float periodic_frequency, int16_t num_samples)
{
float init_value = 500.f;
// tau = 1/(2*pi*Fc)
float tau = 1.0 / (2.0 * M_PI * cutoff_hz);
float sample_time = 1.0 / periodic_frequency;
// Filter init
init_butterworth_2_low_pass(&testfilter, tau, sample_time, init_value);
print_filter_coef(&testfilter);
float sine_freq = 1.f/10.f;
float value = 0.f;
for (int i = 0; i < num_samples; i++)
{
value = sinf(i * 2 * M_PI * sine_freq / periodic_frequency);
float filtered_value = update_butterworth_2_low_pass(&testfilter, value);
printf("value: %f, filtered value: %f\n", value, filtered_value);
}
}
void print_filter_coef(Butterworth2LowPass *filter)
{
printf("filter coefficients and state:\n");
printf("a = %10e, %10e\n", filter->a[0], filter->a[1]);
printf("b = %10e, %10e\n", filter->b[0], filter->b[1]);
printf("i = %f, %f\n", filter->i[0], filter->i[1]);
printf("o = %f, %f\n", filter->o[0], filter->o[1]);
}
void print_filter_values(Butterworth2LowPass *filter)
{
printf("filter values:\n");
printf("i = %f, %f\n", filter->i[0], filter->i[1]);
printf("o = %f, %f\n", filter->o[0], filter->o[1]);
}