From 8c1c99f27f15395c6eed94ab03b88f0289a6d352 Mon Sep 17 00:00:00 2001 From: Oleg Borodin Date: Sat, 10 Sep 2022 16:23:33 +0200 Subject: [PATCH] added lpf filter, pwm output --- mculoop/filter.c | 26 ++++++ mculoop/filter.h | 18 ++++ mculoop/main.c | 212 ++++++++++++++++++++++++++++++++++++++++------ mculoop/misc.c | 18 ++++ mculoop/misc.h | 10 +++ mculoop/pidcont.c | 9 +- 6 files changed, 263 insertions(+), 30 deletions(-) create mode 100644 mculoop/filter.c create mode 100644 mculoop/filter.h create mode 100644 mculoop/misc.c create mode 100644 mculoop/misc.h diff --git a/mculoop/filter.c b/mculoop/filter.c new file mode 100644 index 0000000..7a7c400 --- /dev/null +++ b/mculoop/filter.c @@ -0,0 +1,26 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + +#include +#include + +void lpf_init(lpf_t *lpf, double freq) { + lpf->x0 = 0.0; + lpf->x1 = 0.0; + + double order = 1.5; + double n = 1 / sqrt(pow(2, 1.0 / order) - 1); + lpf->rc = 1 / (2 * n * M_PI * freq); +} + + +double lpf_apply(lpf_t *lpf, double x2, double dt) { + + double k = dt / (lpf->rc + dt); + + lpf->x1 = lpf->x1 + k * (x2 - lpf->x1); + lpf->x0 = lpf->x0 + k * (lpf->x1 - lpf->x0); + return lpf->x0; +} diff --git a/mculoop/filter.h b/mculoop/filter.h new file mode 100644 index 0000000..4db927c --- /dev/null +++ b/mculoop/filter.h @@ -0,0 +1,18 @@ +/* + * Copyright 2022 Oleg Borodin + */ + +#ifndef FILTER_H_QWERTY +#define FILTER_H_QWERTY + + +typedef struct { + double x0; + double x1; + double rc; +} lpf_t; + +void lpf_init(lpf_t *lpf, double freq); +double lpf_apply(lpf_t *lpf, double x2, double dt); + +#endif diff --git a/mculoop/main.c b/mculoop/main.c index 4bc0a99..31a2f1f 100644 --- a/mculoop/main.c +++ b/mculoop/main.c @@ -11,6 +11,8 @@ #include #include #include +#include + #include #include @@ -23,8 +25,12 @@ #include #include #include +#include +#include + + -const uint32_t g_systick_freq = 100 * 1000; +const uint32_t g_systick_freq = 50 * 1000; uint32_t g_sys_tick_counter; //static void _delay(uint32_t n) { @@ -38,7 +44,8 @@ static void clock_setup(void) { rcc_periph_clock_enable(RCC_GPIOB); rcc_periph_clock_enable(RCC_USART1); rcc_periph_clock_enable(RCC_I2C1); - + rcc_periph_clock_enable(RCC_TIM2); + rcc_periph_clock_enable(RCC_TIM5); } static void usart_setup(uint32_t usart, uint32_t gpioport, uint32_t gpiopins, uint32_t baudrate) { @@ -59,6 +66,18 @@ static void usart_setup(uint32_t usart, uint32_t gpioport, uint32_t gpiopins, ui usart_enable(usart); } +static void i2c_setup(uint32_t i2c, uint32_t gpioport, uint32_t gpiopins) { + gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_PULLUP, gpiopins); + gpio_set_output_options(gpioport, GPIO_OTYPE_OD, GPIO_OSPEED_100MHZ, gpiopins); + gpio_set_af(gpioport, GPIO_AF4, gpiopins); + + i2c_reset(i2c); + i2c_peripheral_disable(i2c); + i2c_set_speed(i2c, i2c_speed_fm_400k, I2C_CR2_FREQ_36MHZ); + i2c_peripheral_enable(i2c); +} + + static void systick_setup(uint32_t systic_freq) { g_sys_tick_counter = 0; @@ -70,16 +89,6 @@ static void systick_setup(uint32_t systic_freq) { systick_counter_enable(); } -static void i2c_setup(uint32_t i2c, uint32_t gpioport, uint32_t gpiopins) { - gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_PULLUP, gpiopins); - gpio_set_output_options(gpioport, GPIO_OTYPE_OD, GPIO_OSPEED_100MHZ, gpiopins); - gpio_set_af(gpioport, GPIO_AF4, gpiopins); - - i2c_reset(i2c); - i2c_peripheral_disable(i2c); - i2c_set_speed(i2c, i2c_speed_fm_400k, I2C_CR2_FREQ_36MHZ); - i2c_peripheral_enable(i2c); -} void sys_tick_handler(void) { g_sys_tick_counter++; @@ -91,37 +100,177 @@ uint32_t sys_tick_counter(void) { return val; } + +#define PWM100 9999 +#define PWM150 6666 +#define PWM200 4999 +#define PWM250 3999 +#define PWM300 3333 +#define PWM330 3030 + + +static void timer_init(uint32_t timer) { + + int prescale = rcc_ahb_frequency / (2 * 1000 * 1000) - 1; + int period = PWM330; + + timer_disable_counter(timer); + timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); + timer_disable_preload(timer); + timer_continuous_mode(timer); + timer_set_prescaler(timer, prescale); + timer_set_period(timer, period); + timer_set_repetition_counter(timer, 0); + timer_enable_break_main_output(timer); + timer_enable_counter(timer); +} + +static void tc_init(uint32_t timer, uint32_t channel) { + + timer_disable_oc_output(timer, channel); + timer_set_oc_value(timer, channel, 0); + timer_disable_oc_clear(timer, channel); + timer_enable_oc_preload(timer, channel); + timer_set_oc_slow_mode(timer, channel); + timer_set_oc_mode(timer, channel, TIM_OCM_PWM1); + + timer_set_oc_polarity_high(timer, channel); + timer_set_oc_idle_state_set(timer, channel); + + timer_set_oc_value(timer, channel, 20000); + timer_enable_oc_output(timer, channel); +} + +static void tc_setratio(uint32_t timer, uint32_t channel, uint32_t ratio) { + uint32_t period = TIM_ARR(timer); + uint32_t value = (period * ratio) / 1000; + timer_set_oc_value(timer, channel, value); +} + +static void timer_gpio_setup(uint32_t gpio_port, uint32_t gpio_af, uint32_t gpio_pin) { + gpio_mode_setup(gpio_port, GPIO_MODE_AF, GPIO_PUPD_NONE, gpio_pin); + gpio_set_af(gpio_port, gpio_af, gpio_pin); + gpio_set_output_options(gpio_port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, gpio_pin); +} + + +//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; +} systimer_t; + +void systimer_init(systimer_t* timer, double freq, double start) { + timer->start = start; + timer->freq = freq; +} + +double systimer_next(systimer_t* timer, double timestamp) { + double difftime = (timestamp - timer->start) / timer->freq; + timer->start = timestamp; + return difftime; +} + + int main(void) { clock_setup(); usart_setup(USART1, GPIOA, GPIO9 | GPIO10, 460800); + i2c_setup(I2C1, GPIOB, GPIO8 | GPIO9); systick_setup(g_systick_freq); + timer_gpio_setup(GPIOA, GPIO_AF1, GPIO0); + timer_gpio_setup(GPIOA, GPIO_AF1, GPIO1); + timer_gpio_setup(GPIOA, GPIO_AF1, GPIO2); + timer_gpio_setup(GPIOA, GPIO_AF1, GPIO3); + + uint32_t timer = TIM2; + timer_init(timer); + tc_init(timer, TIM_OC1); + tc_init(timer, TIM_OC2); + tc_init(timer, TIM_OC3); + tc_init(timer, TIM_OC4); + + tc_setratio(timer, TIM_OC1, 10); + tc_setratio(timer, TIM_OC2, 30); + tc_setratio(timer, TIM_OC3, 50); + tc_setratio(timer, TIM_OC4, 70); + + imu_t imu; - printf("==== imu initialize ====\r\n"); imu_setup(&imu, I2C1, 0x68); - imu_calibrate(&imu, 1000); - printf("==== imu started ====\r\n"); + imu_calibrate(&imu, 100); + + printf("start\r\n"); imuvec_t mval; quaternion_t q; quaternion_init(&q); - uint32_t prev_ts = 0; - uint32_t last_ts = 0; - pidcont_t p; pidcont_init(&p); - pidcont_setup(&p, 100, 0, 0); + + 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; + + lpf_init(&lpfax, ak); + lpf_init(&lpfay, ak); + lpf_init(&lpfaz, ak); + + lpf_t lpfgx; + lpf_t lpfgy; + lpf_t lpfgz; + + lpf_init(&lpfgx, ak); + lpf_init(&lpfgy, ak); + lpf_init(&lpfgz, ak); + + systimer_t systimer; + systimer_init(&systimer, (double)g_systick_freq, (double)g_sys_tick_counter); while (true) { imu_getvec(&imu, &mval); - last_ts = g_sys_tick_counter; - double dt = (float)(last_ts - prev_ts) / (float)g_systick_freq; - prev_ts = last_ts; + double dt = systimer_next(&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.gx = lpf_apply(&lpfgx, mval.gx, dt); + mval.gy = lpf_apply(&lpfgy, mval.gy, dt); + mval.gz = lpf_apply(&lpfgz, mval.gz, dt); madgwick(dt, &q, &mval); @@ -129,10 +278,23 @@ int main(void) { quaternion_toeuler(&q, &a); eulerangle_todegress(&a); - //printf("dt=%.6f roll=%8.3f pitch=%8.3f yaw=%8.3f\r\n", dt, a.roll, a.pitch, a.yaw); + double pmin = -90.0; + double pmax = 90.0; + + double omin = 150.0; + double omax = 850.0; + + uint32_t out1 = (uint32_t)mapval(pmin, pmax, omin, omax, a.y, true); + uint32_t out2 = (uint32_t)mapval(pmin, pmax, omin, omax, a.y, false); + + tc_setratio(timer, TIM_OC1, out1); + tc_setratio(timer, TIM_OC2, out2); + + 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); + //printf("dt=%.6f pitch=%8.3f out=%10.3f %10.6f \r\n", dt, a.pitch, out, p.integ); - double out = pidcont_next(&p, 0, a.pitch, dt); - printf("dt=%.6f pitch=%8.3f out=%10.3f \r\n", dt, a.pitch, out); //double b2 = 0.0; //double K = 0.01; diff --git a/mculoop/misc.c b/mculoop/misc.c new file mode 100644 index 0000000..66e82fd --- /dev/null +++ b/mculoop/misc.c @@ -0,0 +1,18 @@ +/* + * Copyright 2022 Oleg Borodin + */ + +#include + +double mapval(double imin, double imax, double omin, double omax, double in, bool inv) { + double odiap = (omax - omin); + double k = 1.0 / (imax - imin); + double out = 0.0; + if (inv) { + out = (omax - odiap/2.0) - in * k * odiap; + } else { + out = in * k * odiap + (odiap/2.0 + omin); + } + + return out; +} diff --git a/mculoop/misc.h b/mculoop/misc.h new file mode 100644 index 0000000..ef48c9a --- /dev/null +++ b/mculoop/misc.h @@ -0,0 +1,10 @@ +/* + * Copyright 2022 Oleg Borodin + */ + +#ifndef MISC_H_QWERTY +#define MISC_H_QWERTY + +double mapval(double imin, double imax, double omin, double omax, double in, bool inv); + +#endif diff --git a/mculoop/pidcont.c b/mculoop/pidcont.c index dbcdb36..4787cba 100644 --- a/mculoop/pidcont.c +++ b/mculoop/pidcont.c @@ -31,13 +31,12 @@ double pidcont_next(pidcont_t* p, double target, double actual, double dt) { double integ = 0.0; double output = 0.0; - const double kk = 100; + error = target - actual; + integ += error * dt; - error = (target - actual); - integ += (error * dt); - deriv = (error - p->perror) * dt; + deriv = error - p->perror; - output = (p->kp * error / kk) + (p->ki * integ / kk) + (p->kd * deriv / kk); + output = (p->kp * error) + (p->ki * integ) + (p->kd * deriv); p->perror = error; p->integ = integ;