added lpf filter, pwm output

mculoop/filter.c

@@ -0,0 +1,26 @@
+/*
+ * Copyright 2022 Oleg Borodin  <borodin@unix7.org>
+ */
+
+
+#include <math.h>
+#include <filter.h>
+
+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;
+}

mculoop/filter.h

@@ -0,0 +1,18 @@
+/*
+ * Copyright 2022 Oleg Borodin  <borodin@unix7.org>
+ */
+
+#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

mculoop/main.c

@@ -11,6 +11,8 @@
 #include <libopencm3/stm32/gpio.h>
 #include <libopencm3/stm32/usart.h>
 #include <libopencm3/stm32/i2c.h>
+#include <libopencm3/stm32/timer.h>
+
 
 #include <stdlib.h>
 #include <stdio.h>
@@ -23,8 +25,12 @@
 #include <geometry.h>
 #include <madgwick.h>
 #include <pidcont.h>
+#include <filter.h>
+#include <misc.h>
 
-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,17 +66,6 @@ static void usart_setup(uint32_t usart, uint32_t gpioport, uint32_t gpiopins, ui
     usart_enable(usart);
 }
 
-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();
-}
-
 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);
@@ -81,6 +77,19 @@ static void i2c_setup(uint32_t i2c, uint32_t gpioport, uint32_t gpiopins) {
     i2c_peripheral_enable(i2c);
 }
 
+
+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();
+}
+
+
 void sys_tick_handler(void) {
     g_sys_tick_counter++;
     //gpio_toggle(GPIOB, GPIO6);
@@ -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);
+    imu_calibrate(&imu, 100);
-    printf("==== imu started ====\r\n");
+
+    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 = systimer_next(&systimer, g_sys_tick_counter);
-        double dt = (float)(last_ts - prev_ts) / (float)g_systick_freq;
+
-        prev_ts = last_ts;
+        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;

mculoop/misc.c

@@ -0,0 +1,18 @@
+/*
+ * Copyright 2022 Oleg Borodin  <borodin@unix7.org>
+ */
+
+#include <stdbool.h>
+
+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;
+}

mculoop/misc.h

@@ -0,0 +1,10 @@
+/*
+ * Copyright 2022 Oleg Borodin  <borodin@unix7.org>
+ */
+
+#ifndef MISC_H_QWERTY
+#define MISC_H_QWERTY
+
+double mapval(double imin, double imax, double omin, double omax, double in, bool inv);
+
+#endif

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);
+    deriv = error - p->perror;
-    integ += (error * dt);
-    deriv = (error - p->perror) * dt;
 
-    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;