added mixer.*;clean olds; added lpf3, etc

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 $@

mculoop/filter.c

@@ -6,7 +6,7 @@
 #include <math.h>
 #include <filter.h>
 
-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;
+}

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);
+void lpf2_init(lpf2_t *lpf, double freq);
-double lpf_apply(lpf_t *lpf, double x2, double dt);
+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

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,6 +196,26 @@ int main(void) {
     quaternion_t q;
     quaternion_init(&q);
 
+    double ak = 50.0;
+    lpf3_t lpfax;
+    lpf3_t lpfay;
+    lpf3_t lpfaz;
+
+    lpf3_init(&lpfax, ak);
+    lpf3_init(&lpfay, ak);
+    lpf3_init(&lpfaz, ak);
+
+    lpf3_t lpfgx;
+    lpf3_t lpfgy;
+    lpf3_t lpfgz;
+
+    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);
 
@@ -238,39 +224,18 @@ int main(void) {
 
     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);
 
-        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.ax = lpf3_apply(&lpfax, mval.ax, dt);
-        mval.ay = lpf_apply(&lpfay, mval.ay, dt);
+        mval.ay = lpf3_apply(&lpfay, mval.ay, dt);
-        mval.az = lpf_apply(&lpfaz, mval.az, dt);
+        mval.az = lpf3_apply(&lpfaz, mval.az, dt);
 
-        mval.gx = lpf_apply(&lpfgx, mval.gx, dt);
+        mval.gx = lpf3_apply(&lpfgx, mval.gx, dt);
-        mval.gy = lpf_apply(&lpfgy, mval.gy, dt);
+        mval.gy = lpf3_apply(&lpfgy, mval.gy, dt);
-        mval.gz = lpf_apply(&lpfgz, mval.gz, 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;
     };
 }

mculoop/pidcont.c

@@ -23,8 +23,7 @@ void pidcont_setup(pidcont_t* p, double kp, double ki, double kd) {
     p->kd = kd;
 }
 
-
+double pidcont_apply(pidcont_t* p, double target, double actual, double dt) {
-double pidcont_next(pidcont_t* p, double target, double actual, double dt) {
 
     double error  = 0.0;
     double deriv  = 0.0;

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