From 35b87dc5e63839ec2e3ad92288db8ac32f7e89ea Mon Sep 17 00:00:00 2001 From: Oleg Borodin Date: Thu, 8 Sep 2022 10:03:53 +0200 Subject: [PATCH] added rapid "pid cont" in pidcont.* --- mculoop/Makefile | 1 + mculoop/geometry.c | 36 ++++++++++++++++++++++-------------- mculoop/geometry.h | 9 ++++----- mculoop/madgwick.c | 10 +++------- mculoop/main.c | 14 ++++++++++---- mculoop/pidcont.c | 45 +++++++++++++++++++++++++++++++++++++++++++++ mculoop/pidcont.h | 25 +++++++++++++++++++++++++ 7 files changed, 110 insertions(+), 30 deletions(-) create mode 100644 mculoop/pidcont.c create mode 100644 mculoop/pidcont.h diff --git a/mculoop/Makefile b/mculoop/Makefile index 070253c..d2befe7 100644 --- a/mculoop/Makefile +++ b/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 $@ diff --git a/mculoop/geometry.c b/mculoop/geometry.c index 6d6fb93..6cc623f 100644 --- a/mculoop/geometry.c +++ b/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; } diff --git a/mculoop/geometry.h b/mculoop/geometry.h index 1659334..9886f50 100644 --- a/mculoop/geometry.h +++ b/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 diff --git a/mculoop/madgwick.c b/mculoop/madgwick.c index 352268c..cb8066b 100644 --- a/mculoop/madgwick.c +++ b/mculoop/madgwick.c @@ -8,10 +8,6 @@ #include -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; diff --git a/mculoop/main.c b/mculoop/main.c index cf71c14..4bc0a99 100644 --- a/mculoop/main.c +++ b/mculoop/main.c @@ -22,7 +22,7 @@ #include #include #include - +#include 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; diff --git a/mculoop/pidcont.c b/mculoop/pidcont.c new file mode 100644 index 0000000..dbcdb36 --- /dev/null +++ b/mculoop/pidcont.c @@ -0,0 +1,45 @@ +/* + * Copyright 2022 Oleg Borodin + * + */ + + +#include +#include + +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; +} diff --git a/mculoop/pidcont.h b/mculoop/pidcont.h new file mode 100644 index 0000000..353b622 --- /dev/null +++ b/mculoop/pidcont.h @@ -0,0 +1,25 @@ +/* + * Copyright 2022 Oleg Borodin + * + */ + + +#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