mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-07 09:36:19 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -8,6 +8,9 @@ VOR_LF_SRCS = main.c \
|
||||
vor_lf_filter.c \
|
||||
|
||||
|
||||
test_123: test_123.c vor_filter.c vor_filter_params.c
|
||||
gcc $(CFLAGS) $^ -o $@ $(LDFLAGS)
|
||||
|
||||
vor_lf: $(VOR_LF_SRCS)
|
||||
gcc $(CFLAGS) $^ -o $@ $(LDFLAGS)
|
||||
|
||||
|
||||
@@ -5,12 +5,14 @@ in_float = M(:,2);
|
||||
out_float = M(:,3);
|
||||
in_int = M(:,4);
|
||||
out_int = M(:,5);
|
||||
out_int_f = M(:,6);
|
||||
|
||||
clf();
|
||||
drawlater
|
||||
subplot(2,1,1)
|
||||
plot2d(time, in_float, 2);
|
||||
plot2d(time, out_float, 3);
|
||||
plot2d(time, out_int_f, 5);
|
||||
|
||||
subplot(2,1,2)
|
||||
plot2d(time, in_int, 2);
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
#include <stdio.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
#include "vor_integer_filters.h"
|
||||
#include "vor_float_filters.h"
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
int i;
|
||||
|
||||
for(i=0; i<400; i++) {
|
||||
float xn_f = 0.9; //!i ? .9 : 0.;
|
||||
uint16_t xn_i = xn_f * VIF_SFACT;
|
||||
|
||||
//float yn_f = vor_float_filter_bp_var(xn_f);
|
||||
//int32_t yn_i = vor_int_filter_bp_var(xn_i);
|
||||
|
||||
float yn_f = vor_float_filter_bp_ref(xn_f);
|
||||
int32_t yn_i = vor_int_filter_bp_ref(xn_i);
|
||||
|
||||
printf("%d\t%f\t%f\t%d\t%d\t%f\n",
|
||||
i, xn_f, yn_f, xn_i, yn_i, (float)yn_i / (float)VIF_SFACT);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -5,10 +5,8 @@
|
||||
#include <inttypes.h>
|
||||
|
||||
|
||||
#include "vor_filter_params.h"
|
||||
#include "vor_filter.h"
|
||||
#include "vor_integer_filters.h"
|
||||
|
||||
#include "vor_float_filters.h"
|
||||
|
||||
|
||||
static short* wav_buf;
|
||||
@@ -31,7 +29,7 @@ static void read_wave(const char* filename) {
|
||||
int i;
|
||||
for (i=0; i<nb_samples; i++) {
|
||||
float_buf[i] = (float)wav_buf[i] / (float)(1<<15);
|
||||
adc_buf[i] = float_buf[i] * 1024 * (1<<5);
|
||||
adc_buf[i] = float_buf[i] * VIF_SFACT * (1<<5);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -43,21 +41,20 @@ int main(int argc, char** argv) {
|
||||
|
||||
read_wave("signal_VOR_BF_50_200dB.wav");
|
||||
|
||||
struct VorFilter vor_filter_bp_var;
|
||||
vor_filter_init(&vor_filter_bp_var,
|
||||
BP_VAR_NUM_LEN, BP_VAR_DEN_LEN,
|
||||
BP_VAR_NUM, BP_VAR_DEN);
|
||||
|
||||
int i;
|
||||
float te = 1/29880.;
|
||||
|
||||
for (i=0; i<nb_samples; i++) {
|
||||
float t = i * te;
|
||||
float yi_f = vor_filter_run(&vor_filter_bp_var, float_buf[i]);
|
||||
int32_t yi_i = filter_bp_var(adc_buf[i]);
|
||||
// yi_i = yi_i>>21;
|
||||
// float yi_f = vor_float_filter_bp_var(float_buf[i]);
|
||||
// int32_t yi_i = vor_int_filter_bp_var(adc_buf[i]);
|
||||
|
||||
printf("%f %f %f %d %d\n", t, float_buf[i], yi_f, adc_buf[i], yi_i);
|
||||
float yi_f = vor_float_filter_bp_ref(float_buf[i]);
|
||||
int32_t yi_i = vor_int_filter_bp_ref(adc_buf[i]);
|
||||
|
||||
|
||||
printf("%f\t%f\t%f\t%d\t%d\t%f\n",
|
||||
t, float_buf[i], yi_f, adc_buf[i], yi_i, (float)yi_i / (float)VIF_SFACT / (1<<5));
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
@@ -0,0 +1,108 @@
|
||||
#ifndef VOR_FLOAT_FILTERS_H
|
||||
#define VOR_FLOAT_FILTERS_H
|
||||
|
||||
inline float vor_float_filter_bp_var( float xn) {
|
||||
|
||||
const float a0 = 0.0000294918571461433008684162315748977790;
|
||||
const float a1 = 0.0000884755714384299093815122727590960494;
|
||||
const float a2 = 0.0000884755714384299093815122727590960494;
|
||||
const float a3 = 0.0000294918571461433008684162315748977790;
|
||||
|
||||
const float b1 = -2.8738524677701420273479016032069921493530;
|
||||
const float b2 = 2.7555363566291544152875303552718833088875;
|
||||
const float b3 = -0.8814479540018430592240861187747213989496;
|
||||
|
||||
static float x1 = 0;
|
||||
static float x2 = 0;
|
||||
static float x3 = 0;
|
||||
|
||||
static float y1 = 0;
|
||||
static float y2 = 0;
|
||||
static float y3 = 0;
|
||||
|
||||
float yn = a0 * xn
|
||||
+ a1 * x1
|
||||
+ a2 * x2
|
||||
+ a3 * x3
|
||||
- b1 * y1
|
||||
- b2 * y2
|
||||
- b3 * y3;
|
||||
|
||||
x3 = x2;
|
||||
x2 = x1;
|
||||
x1 = xn;
|
||||
|
||||
y3 = y2;
|
||||
y2 = y1;
|
||||
y1 = yn;
|
||||
|
||||
return yn;
|
||||
}
|
||||
|
||||
inline float vor_float_filter_bp_ref( float xn) {
|
||||
|
||||
const float a0 = 0.0175489156490784836694984960558940656483;
|
||||
// const float a1 = 0.0000000000000000000000000000000000000000;
|
||||
const float a2 = -0.0526467469472354510084954881676821969450;
|
||||
// const float a3 = 0.0000000000000000000000000000000000000000;
|
||||
const float a4 = 0.0526467469472354510084954881676821969450;
|
||||
// const float a5 = 0.0000000000000000000000000000000000000000;
|
||||
const float a6 = -0.0175489156490784836694984960558940656483;
|
||||
|
||||
const float b1 = 2.5508195725874163173330089193768799304962;
|
||||
const float b2 = 3.9857308274503626677187639870680868625641;
|
||||
const float b3 = 3.8245798569419009460546021728077903389931;
|
||||
const float b4 = 2.6296760724926846464200025366153568029404;
|
||||
const float b5 = 1.0926715614934312537087635064381174743176;
|
||||
const float b6 = 0.2825578543465128156242371915141120553017;
|
||||
|
||||
static float x1 = 0;
|
||||
static float x2 = 0;
|
||||
static float x3 = 0;
|
||||
static float x4 = 0;
|
||||
static float x5 = 0;
|
||||
static float x6 = 0;
|
||||
|
||||
static float y1 = 0;
|
||||
static float y2 = 0;
|
||||
static float y3 = 0;
|
||||
static float y4 = 0;
|
||||
static float y5 = 0;
|
||||
static float y6 = 0;
|
||||
|
||||
float yn = a0 * xn
|
||||
// + a1 * x1
|
||||
+ a2 * x2
|
||||
// + a3 * x3
|
||||
+ a4 * x4
|
||||
// + a5 * x5
|
||||
+ a6 * x6
|
||||
- b1 * y1
|
||||
- b2 * y2
|
||||
- b3 * y3
|
||||
- b4 * y4
|
||||
- b5 * y5
|
||||
- b6 * y6;
|
||||
|
||||
x6 = x5;
|
||||
x5 = x4;
|
||||
x4 = x3;
|
||||
x3 = x2;
|
||||
x2 = x1;
|
||||
x1 = xn;
|
||||
|
||||
y6 = y5;
|
||||
y5 = y4;
|
||||
y4 = y3;
|
||||
y3 = y2;
|
||||
y2 = y1;
|
||||
y1 = yn;
|
||||
|
||||
return yn;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif /* VOR_FLOAT_FILTERS_H */
|
||||
@@ -0,0 +1,132 @@
|
||||
#ifndef VOR_INTEGER_FILTERS_H
|
||||
#define VOR_INTEGER_FILTERS_H
|
||||
|
||||
|
||||
#define VIF_CRES 16
|
||||
#define VIF_CFACT (1<<VIF_CRES)
|
||||
#define VIF_PCOEF(a) (a * VIF_CFACT + 0.5)
|
||||
#define VIF_NCOEF(a) (a * VIF_CFACT - 0.5)
|
||||
|
||||
#define VIF_SRES 10
|
||||
#define VIF_SFACT (1<<VIF_SRES)
|
||||
|
||||
#define VIF_SSCALE 5
|
||||
|
||||
inline int32_t vor_int_filter_bp_var( uint16_t xn) {
|
||||
|
||||
const int32_t a0 = VIF_PCOEF( 0.0000294918571461433008684162315748977790);
|
||||
const int32_t a1 = VIF_PCOEF( 0.0000884755714384299093815122727590960494);
|
||||
const int32_t a2 = VIF_PCOEF( 0.0000884755714384299093815122727590960494);
|
||||
const int32_t a3 = VIF_PCOEF( 0.0000294918571461433008684162315748977790);
|
||||
|
||||
const int32_t b1 = VIF_NCOEF(-2.8738524677701420273479016032069921493530);
|
||||
const int32_t b2 = VIF_PCOEF( 2.7555363566291544152875303552718833088875);
|
||||
const int32_t b3 = VIF_NCOEF(-0.8814479540018430592240861187747213989496);
|
||||
|
||||
// printf("%d %d %d %d\n", a0, a1, a2, a3);
|
||||
// printf("%d %d %d\n", b1, b2, b3);
|
||||
|
||||
static uint16_t x1 = 0;
|
||||
static uint16_t x2 = 0;
|
||||
static uint16_t x3 = 0;
|
||||
|
||||
static int32_t y1 = 0;
|
||||
static int32_t y2 = 0;
|
||||
static int32_t y3 = 0;
|
||||
|
||||
/* scale input */
|
||||
xn = (xn << VIF_SSCALE);
|
||||
|
||||
int32_t yn = a0 * xn
|
||||
+ a1 * x1
|
||||
+ a2 * x2
|
||||
+ a3 * x3
|
||||
- b1 * y1
|
||||
- b2 * y2
|
||||
- b3 * y3;
|
||||
// printf("%d\n", yn);
|
||||
yn = (yn >> VIF_CRES);
|
||||
|
||||
x3 = x2;
|
||||
x2 = x1;
|
||||
x1 = xn;
|
||||
|
||||
y3 = y2;
|
||||
y2 = y1;
|
||||
y1 = yn;
|
||||
|
||||
return (yn >> VIF_SSCALE);
|
||||
}
|
||||
|
||||
inline int32_t vor_int_filter_bp_ref( uint16_t xn) {
|
||||
|
||||
const int32_t a0 = VIF_PCOEF( 0.0175489156490784836694984960558940656483);
|
||||
// const int32_t a1 = VIF_PCOEF( 0.0000000000000000000000000000000000000000);
|
||||
const int32_t a2 = VIF_NCOEF(-0.0526467469472354510084954881676821969450);
|
||||
// const int32_t a3 = VIF_PCOEF( 0.0000000000000000000000000000000000000000);
|
||||
const int32_t a4 = VIF_PCOEF( 0.0526467469472354510084954881676821969450);
|
||||
// const int32_t a5 = VIF_PCOEF( 0.0000000000000000000000000000000000000000);
|
||||
const int32_t a6 = VIF_NCOEF(-0.0175489156490784836694984960558940656483);
|
||||
|
||||
const int32_t b1 = VIF_PCOEF( 2.5508195725874163173330089193768799304962);
|
||||
const int32_t b2 = VIF_PCOEF( 3.9857308274503626677187639870680868625641);
|
||||
const int32_t b3 = VIF_PCOEF( 3.8245798569419009460546021728077903389931);
|
||||
const int32_t b4 = VIF_PCOEF( 2.6296760724926846464200025366153568029404);
|
||||
const int32_t b5 = VIF_PCOEF( 1.0926715614934312537087635064381174743176);
|
||||
const int32_t b6 = VIF_PCOEF( 0.2825578543465128156242371915141120553017);
|
||||
|
||||
// printf("%d %d %d %d %d %d %d\n", a0, a1, a2, a3, a4, a5, a6);
|
||||
// printf("%d %d %d %d %d %d\n", b1, b2, b3, b4, b5, b6);
|
||||
|
||||
static uint16_t x1 = 0;
|
||||
static uint16_t x2 = 0;
|
||||
static uint16_t x3 = 0;
|
||||
static uint16_t x4 = 0;
|
||||
static uint16_t x5 = 0;
|
||||
static uint16_t x6 = 0;
|
||||
|
||||
static int32_t y1 = 0;
|
||||
static int32_t y2 = 0;
|
||||
static int32_t y3 = 0;
|
||||
static int32_t y4 = 0;
|
||||
static int32_t y5 = 0;
|
||||
static int32_t y6 = 0;
|
||||
|
||||
/* scale input */
|
||||
xn = (xn << VIF_SSCALE);
|
||||
|
||||
int32_t yn = a0 * xn
|
||||
// + a1 * x1
|
||||
+ a2 * x2
|
||||
// + a3 * x3
|
||||
+ a4 * x4
|
||||
// + a5 * x5
|
||||
+ a6 * x6
|
||||
- b1 * y1
|
||||
- b2 * y2
|
||||
- b3 * y3
|
||||
- b4 * y4
|
||||
- b5 * y5
|
||||
- b6 * y6;
|
||||
// printf("%d\n", yn);
|
||||
yn = (yn >> VIF_CRES);
|
||||
|
||||
x6 = x5;
|
||||
x5 = x4;
|
||||
x4 = x3;
|
||||
x3 = x2;
|
||||
x2 = x1;
|
||||
x1 = xn;
|
||||
|
||||
y6 = y5;
|
||||
y5 = y4;
|
||||
y4 = y3;
|
||||
y3 = y2;
|
||||
y2 = y1;
|
||||
y1 = yn;
|
||||
|
||||
return (yn >> VIF_SSCALE);
|
||||
}
|
||||
|
||||
|
||||
#endif /* VOR_INTEGER_FILTERS_H */
|
||||
Reference in New Issue
Block a user