From 13f9b3f8de93f4a2981f6595daa004652fe29c67 Mon Sep 17 00:00:00 2001 From: Oleg Borodin Date: Sat, 10 Sep 2022 22:38:13 +0200 Subject: [PATCH] added mixer.*;clean olds; added lpf3, etc --- mculoop/Makefile | 2 + mculoop/filter.c | 25 +++++++++++- mculoop/filter.h | 17 +++++++-- mculoop/main.c | 96 ++++++++++++++--------------------------------- mculoop/pidcont.c | 3 +- mculoop/pidcont.h | 2 +- 6 files changed, 69 insertions(+), 76 deletions(-) diff --git a/mculoop/Makefile b/mculoop/Makefile index d2befe7..62e4a0f 100644 --- a/mculoop/Makefile +++ b/mculoop/Makefile @@ -35,6 +35,8 @@ OBJS+= geometry.o OBJS+= madgwick.o OBJS+= i2cdev.o OBJS+= pidcont.o +OBJS+= mixer.o + main.elf: $(OBJS) $(TARGET)-gcc $(^F) $(LDFLAGS) -o $@ diff --git a/mculoop/filter.c b/mculoop/filter.c index 7a7c400..e368694 100644 --- a/mculoop/filter.c +++ b/mculoop/filter.c @@ -6,7 +6,7 @@ #include #include -void lpf_init(lpf_t *lpf, double freq) { +void lpf2_init(lpf2_t *lpf, double freq) { lpf->x0 = 0.0; lpf->x1 = 0.0; @@ -16,7 +16,7 @@ void lpf_init(lpf_t *lpf, double freq) { } -double lpf_apply(lpf_t *lpf, double x2, double dt) { +double lpf2_apply(lpf2_t *lpf, double x2, double dt) { double k = dt / (lpf->rc + dt); @@ -24,3 +24,24 @@ double lpf_apply(lpf_t *lpf, double x2, double dt) { lpf->x0 = lpf->x0 + k * (lpf->x1 - lpf->x0); return lpf->x0; } + + +void lpf3_init(lpf3_t *lpf, double freq) { + lpf->x1 = 0.0; + lpf->x2 = 0.0; + lpf->x3 = 0.0; + + double order = 2.0; + double c = 1.0 / sqrt(powf(2.0, 1.0 / order) - 1.0); + lpf->rc = 1.0 / (2.0 * c * M_PI * freq); +} + +double lpf3_apply(lpf3_t *lpf, double x0, double dt) { + + double k = dt / (lpf->rc + dt); + + lpf->x1 = lpf->x1 + k * (x0 - lpf->x1); + lpf->x2 = lpf->x2 + k * (lpf->x1 - lpf->x2); + lpf->x3 = lpf->x3 + k * (lpf->x2 - lpf->x3); + return lpf->x3; +} diff --git a/mculoop/filter.h b/mculoop/filter.h index 4db927c..beb9cb4 100644 --- a/mculoop/filter.h +++ b/mculoop/filter.h @@ -10,9 +10,20 @@ typedef struct { double x0; double x1; double rc; -} lpf_t; +} lpf2_t; -void lpf_init(lpf_t *lpf, double freq); -double lpf_apply(lpf_t *lpf, double x2, double dt); +void lpf2_init(lpf2_t *lpf, double freq); +double lpf2_apply(lpf2_t *lpf, double x, double dt); + + +typedef struct { + double x1; + double x2; + double x3; + double rc; +} lpf3_t; + +void lpf3_init(lpf3_t *lpf, double freq); +double lpf3_apply(lpf3_t *lpf, double x, double dt); #endif diff --git a/mculoop/main.c b/mculoop/main.c index 31a2f1f..f1d55b9 100644 --- a/mculoop/main.c +++ b/mculoop/main.c @@ -33,11 +33,6 @@ const uint32_t g_systick_freq = 50 * 1000; uint32_t g_sys_tick_counter; -//static void _delay(uint32_t n) { -// for (int i = 0; i < n * ; i++) -// __asm__("nop"); -//} - static void clock_setup(void) { rcc_clock_setup_pll(&rcc_hse_8mhz_3v3[RCC_CLOCK_3V3_168MHZ]); rcc_periph_clock_enable(RCC_GPIOA); @@ -81,9 +76,6 @@ static void i2c_setup(uint32_t i2c, uint32_t gpioport, uint32_t gpiopins) { static void systick_setup(uint32_t systic_freq) { g_sys_tick_counter = 0; - //gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO6); - //gpio_set_output_options(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_100MHZ, GPIO6); - systick_set_frequency(systic_freq, rcc_ahb_frequency); systick_interrupt_enable(); systick_counter_enable(); @@ -92,7 +84,6 @@ static void systick_setup(uint32_t systic_freq) { void sys_tick_handler(void) { g_sys_tick_counter++; - //gpio_toggle(GPIOB, GPIO6); } uint32_t sys_tick_counter(void) { @@ -108,7 +99,6 @@ uint32_t sys_tick_counter(void) { #define PWM300 3333 #define PWM330 3030 - static void timer_init(uint32_t timer) { int prescale = rcc_ahb_frequency / (2 * 1000 * 1000) - 1; @@ -154,29 +144,6 @@ static void timer_gpio_setup(uint32_t gpio_port, uint32_t gpio_af, uint32_t gpio } -//typedef struct { -// double k; -// double l; -//} lpf_t; - - -//void lpf_init(lpf_t* lpf, double k) { -// lpf->k = k; -// lpf->l = 0; -//} - - -//double simplp (double *x, double *y, int M, double xm1) { -// int n; -// y[0] = x[0] + xm1; -// for (n = 1; n < M ; n++) { -// y[n] = x[n] + x[n-1]; -// } -// return x[M-1]; -//} - - - typedef struct { double start; double freq; @@ -187,7 +154,7 @@ void systimer_init(systimer_t* timer, double freq, double start) { timer->freq = freq; } -double systimer_next(systimer_t* timer, double timestamp) { +double systimer_apply(systimer_t* timer, double timestamp) { double difftime = (timestamp - timer->start) / timer->freq; timer->start = timestamp; return difftime; @@ -219,7 +186,6 @@ int main(void) { tc_setratio(timer, TIM_OC3, 50); tc_setratio(timer, TIM_OC4, 70); - imu_t imu; imu_setup(&imu, I2C1, 0x68); imu_calibrate(&imu, 100); @@ -230,47 +196,46 @@ int main(void) { quaternion_t q; quaternion_init(&q); - pidcont_t p; - pidcont_init(&p); - - double kp = 0; - double ki = 4000.0; - - pidcont_setup(&p, kp, ki, 0); - double ak = 50.0; - lpf_t lpfax; - lpf_t lpfay; - lpf_t lpfaz; + lpf3_t lpfax; + lpf3_t lpfay; + lpf3_t lpfaz; - lpf_init(&lpfax, ak); - lpf_init(&lpfay, ak); - lpf_init(&lpfaz, ak); + lpf3_init(&lpfax, ak); + lpf3_init(&lpfay, ak); + lpf3_init(&lpfaz, ak); - lpf_t lpfgx; - lpf_t lpfgy; - lpf_t lpfgz; + lpf3_t lpfgx; + lpf3_t lpfgy; + lpf3_t lpfgz; - lpf_init(&lpfgx, ak); - lpf_init(&lpfgy, ak); - lpf_init(&lpfgz, ak); + lpf3_init(&lpfgx, ak); + lpf3_init(&lpfgy, ak); + lpf3_init(&lpfgz, ak); systimer_t systimer; systimer_init(&systimer, (double)g_systick_freq, (double)g_sys_tick_counter); + pidcont_t p; + pidcont_init(&p); + + double kp = 0; + double ki = 4000.0; + + pidcont_setup(&p, kp, ki, 0); while (true) { imu_getvec(&imu, &mval); - double dt = systimer_next(&systimer, g_sys_tick_counter); + double dt = systimer_apply(&systimer, g_sys_tick_counter); - mval.ax = lpf_apply(&lpfax, mval.ax, dt); - mval.ay = lpf_apply(&lpfay, mval.ay, dt); - mval.az = lpf_apply(&lpfaz, mval.az, dt); + mval.ax = lpf3_apply(&lpfax, mval.ax, dt); + mval.ay = lpf3_apply(&lpfay, mval.ay, dt); + mval.az = lpf3_apply(&lpfaz, mval.az, dt); - mval.gx = lpf_apply(&lpfgx, mval.gx, dt); - mval.gy = lpf_apply(&lpfgy, mval.gy, dt); - mval.gz = lpf_apply(&lpfgz, mval.gz, dt); + mval.gx = lpf3_apply(&lpfgx, mval.gx, dt); + mval.gy = lpf3_apply(&lpfgy, mval.gy, dt); + mval.gz = lpf3_apply(&lpfgz, mval.gz, dt); madgwick(dt, &q, &mval); @@ -292,12 +257,7 @@ int main(void) { printf("dt=%.6f %lu <-pitch=%8.3f roll=%8.3f yaw=%8.3f\r\n", dt, out1, a.y, a.x, a.z); - //double out = pidcont_next(&p, 0, a.pitch, dt); + //double out = pidcont_apply(&p, 0, a.pitch, dt); //printf("dt=%.6f pitch=%8.3f out=%10.3f %10.6f \r\n", dt, a.pitch, out, p.integ); - - - //double b2 = 0.0; - //double K = 0.01; - //b2 = b2*(1 - K) + b*K; }; } diff --git a/mculoop/pidcont.c b/mculoop/pidcont.c index 4787cba..ef7d6bf 100644 --- a/mculoop/pidcont.c +++ b/mculoop/pidcont.c @@ -23,8 +23,7 @@ void pidcont_setup(pidcont_t* p, double kp, double ki, double kd) { p->kd = kd; } - -double pidcont_next(pidcont_t* p, double target, double actual, double dt) { +double pidcont_apply(pidcont_t* p, double target, double actual, double dt) { double error = 0.0; double deriv = 0.0; diff --git a/mculoop/pidcont.h b/mculoop/pidcont.h index 353b622..4a7c516 100644 --- a/mculoop/pidcont.h +++ b/mculoop/pidcont.h @@ -20,6 +20,6 @@ typedef struct { void pidcont_init(pidcont_t* p); void pidcont_setup(pidcont_t* p, double kp, double ki, double kd); -double pidcont_next(pidcont_t* p, double target, double actual, double dt); +double pidcont_apply(pidcont_t* p, double target, double actual, double dt); #endif