added rapid "pid cont" in pidcont.*

mculoop/Makefile

@@ -34,6 +34,7 @@ OBJS+= mpu6050.o
 OBJS+= geometry.o
 OBJS+= madgwick.o
 OBJS+= i2cdev.o
+OBJS+= pidcont.o
 
 main.elf: $(OBJS)
 	$(TARGET)-gcc $(^F) $(LDFLAGS) -o $@

mculoop/geometry.c

@@ -34,10 +34,10 @@ void eulerangle_norm(eulerangle_t* a) {
 }
 
 void quaternion_init(quaternion_t* q) {
-    q->w = 1.0f;
-    q->x = 0.0f;
-    q->y = 0.0f;
-    q->z = 0.0f;
+    q->w = 1.0;
+    q->x = 0.0;
+    q->y = 0.0;
+    q->z = 0.0;
 }
 
 void quaternion_toeuler(quaternion_t* q, eulerangle_t* a) {
@@ -47,22 +47,30 @@ void quaternion_toeuler(quaternion_t* q, eulerangle_t* a) {
     double z = q->z;
     double w = q->w;
 
+    double ax = 0.0;
+    double ay = 0.0;
+    double az = 0.0;
+
     double t0 = (x + z)*(x - z);     // x^2-z^2
     double t1 = (w + y)*(w - y);     // w^2-y^2
 
-    double xx = 0.5 * (t0 + t1);     // 1/2 x of x'
-    double xy = x*y + w*z;           // 1/2 y of x'
-    double xz = w*y - x*z;           // 1/2 z of x'
+    double n1 = 0.5 * (t0 + t1);     // 1/2 x of x'
+    double n2 = x*y + w*z;           // 1/2 y of x'
+    double n3 = w*y - x*z;           // 1/2 z of x'
 
-    double t  = xx*xx + xy*xy;       // cos(theta)^2
-    double yz = 2.0 * (y*z + w*x);   // z of y'
+    double t  = n1*n1 + n2*n2;       // cos(theta)^2
+    double n4 = 2.0 * (y*z + w*x);   // z of y'
 
-    a->z = atan2(xy, xx);             // yaw   (psi)
-    a->y = atan(xz /sqrt(t));         // pitch (theta)
+    az = atan2(n2, n1);              // yaw   (psi)
+    ay = atan(n3 / sqrt(t));         // pitch (theta)
 
-    if (t != 0) {
-        a->x = atan2(yz, t1 - t0);
+    if (t != 0.0) {                  // roll
+        ax = atan2(n4, t1 - t0);
     } else {
-        a->x = (2.0*atan2(x, w) - copysign(1.0, xz)*a->z);
+        ax = (2.0 * atan2(x, w) - copysign(1.0, n3) * az);
     }
+
+    a->x = ax;
+    a->y = ay;
+    a->z = az;
 }

mculoop/geometry.h

@@ -13,10 +13,10 @@ typedef struct {
 
 
 typedef struct quaternion_s {
-    double w;
-    double x;
-    double y;
-    double z;
+    union { double w; double q0; };
+    union { double x; double q1; };
+    union { double y; double q2; };
+    union { double z; double q3; };
 } quaternion_t;
 
 
@@ -37,5 +37,4 @@ void eulerangle_toradians(eulerangle_t* a);
 void quaternion_init(quaternion_t* q);
 void quaternion_toeuler(quaternion_t* q, eulerangle_t* a);
 
-
 #endif

mculoop/madgwick.c

@@ -8,10 +8,6 @@
 
 #include <geometry.h>
 
-static double inv_sqrt(double x) {
-    return 1.0 / sqrt(x);
-}
-
 void madgwick(double dt, quaternion_t* q, imuvec_t* m) {
 
     double q0 = q->w;
@@ -42,7 +38,7 @@ void madgwick(double dt, quaternion_t* q, imuvec_t* m) {
     if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) {
 
         // Normalise accelerometer measurement
-        recipNorm = inv_sqrt(ax*ax + ay*ay + az*az);
+        recipNorm = 1.0 / sqrt(ax*ax + ay*ay + az*az);
         ax *= recipNorm;
         ay *= recipNorm;
         az *= recipNorm;
@@ -54,7 +50,7 @@ void madgwick(double dt, quaternion_t* q, imuvec_t* m) {
         s3 = 4.0*q1*q1*q3 - 2.0*q1*ax + 4.0*q2*q2*q3 - 2.0*q2*ay;
 
         // Normalise step magnitude
-        recipNorm = inv_sqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3);
+        recipNorm = 1.0 / sqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3);
         s0 *= recipNorm;
         s1 *= recipNorm;
         s2 *= recipNorm;
@@ -74,7 +70,7 @@ void madgwick(double dt, quaternion_t* q, imuvec_t* m) {
     q3 += qDot4 * dt;
 
     // Normalise quaternion
-    recipNorm = inv_sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
+    recipNorm = 1.0 / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
     q0 *= recipNorm;
     q1 *= recipNorm;
     q2 *= recipNorm;

mculoop/main.c

@@ -22,7 +22,7 @@
 #include <mpu6050.h>
 #include <geometry.h>
 #include <madgwick.h>
-
+#include <pidcont.h>
 
 const uint32_t g_systick_freq = 100 * 1000;
 uint32_t g_sys_tick_counter;
@@ -101,7 +101,7 @@ int main(void) {
     imu_t imu;
     printf("==== imu initialize ====\r\n");
     imu_setup(&imu, I2C1, 0x68);
-    imu_calibrate(&imu, 20000);
+    imu_calibrate(&imu, 1000);
     printf("==== imu started ====\r\n");
 
     imuvec_t mval;
@@ -111,7 +111,10 @@ int main(void) {
     uint32_t prev_ts = 0;
     uint32_t last_ts = 0;
 
-    printf("==== main loop ====\r\n");
+    pidcont_t p;
+    pidcont_init(&p);
+    pidcont_setup(&p, 100, 0, 0);
+
 
     while (true) {
         imu_getvec(&imu, &mval);
@@ -126,7 +129,10 @@ int main(void) {
         quaternion_toeuler(&q, &a);
         eulerangle_todegress(&a);
 
-        printf("dt=%.6f y=%10.4f x=%10.4f z=%10.4f  \r\n", dt, a.y, a.x, a.z);
+        //printf("dt=%.6f roll=%8.3f  pitch=%8.3f  yaw=%8.3f\r\n", dt, a.roll, a.pitch, a.yaw);
+
+        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/pidcont.c

@@ -0,0 +1,45 @@
+/*
+ * Copyright 2022 Oleg Borodin  <borodin@unix7.org>
+ *
+ */
+
+
+#include <stdint.h>
+#include <pidcont.h>
+
+void pidcont_init(pidcont_t* p) {
+    p->perror = 0.0;
+    p->integ = 0.0;
+
+    p->kp = 0.0;
+    p->ki = 0.0;
+    p->kd = 0.0;
+}
+
+
+void pidcont_setup(pidcont_t* p, double kp, double ki, double kd) {
+    p->kp = kp;
+    p->ki = ki;
+    p->kd = kd;
+}
+
+
+double pidcont_next(pidcont_t* p, double target, double actual, double dt) {
+
+    double error  = 0.0;
+    double deriv  = 0.0;
+    double integ  = 0.0;
+    double output = 0.0;
+
+    const double kk = 100;
+
+    error = (target - actual);
+    integ += (error * dt);
+    deriv = (error - p->perror) * dt;
+
+    output = (p->kp * error / kk) + (p->ki * integ / kk) + (p->kd * deriv / kk);
+
+    p->perror = error;
+    p->integ  = integ;
+    return output;
+}

mculoop/pidcont.h

@@ -0,0 +1,25 @@
+/*
+ * Copyright 2022 Oleg Borodin  <borodin@unix7.org>
+ *
+ */
+
+
+#ifndef PIDCONT_H_QWERTY
+#define PIDCONT_H_QWERTY
+
+
+typedef struct {
+    double perror;
+    double integ;
+
+    double kp;
+    double ki;
+    double kd;
+} pidcont_t;
+
+
+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);
+
+#endif