From 8e211fd8fb135b1ba97a01b4698f57d49825fbc3 Mon Sep 17 00:00:00 2001 From: Oleg Borodin Date: Tue, 27 Sep 2022 12:43:59 +0200 Subject: [PATCH] madgwick() moved to geometry.c --- mainloop/Makefile | 1 - mainloop/geometry.c | 75 ++++++++++++++++++++++++++++++++++++++++ mainloop/geometry.h | 2 ++ mainloop/madgwick.c | 83 --------------------------------------------- mainloop/madgwick.h | 13 ------- mainloop/main.c | 10 ++---- 6 files changed, 79 insertions(+), 105 deletions(-) delete mode 100644 mainloop/madgwick.c delete mode 100644 mainloop/madgwick.h diff --git a/mainloop/Makefile b/mainloop/Makefile index 4ed21d1..4a56bdc 100644 --- a/mainloop/Makefile +++ b/mainloop/Makefile @@ -35,7 +35,6 @@ OBJS+= syscall.o OBJS+= usartu.o OBJS+= mpu6050.o OBJS+= geometry.o -OBJS+= madgwick.o OBJS+= i2cdev.o OBJS+= pidcont.o OBJS+= mixer.o diff --git a/mainloop/geometry.c b/mainloop/geometry.c index 6cc623f..89f84e1 100644 --- a/mainloop/geometry.c +++ b/mainloop/geometry.c @@ -74,3 +74,78 @@ void quaternion_toeuler(quaternion_t* q, eulerangle_t* a) { a->y = ay; a->z = az; } + + +void quaternion_madgwick(quaternion_t* q, imuvec_t* m, double dt) { + + double q0 = q->w; + double q1 = q->x; + double q2 = q->y; + double q3 = q->z; // quaternion of sensor frame relative to auxiliary frame + + double gx = m->gx; + double gy = m->gy; + double gz = m->gz; + + double ax = m->ax; + double ay = m->ay; + double az = m->az; + + double beta = 0.1f; // 2 * proportional gain (Kp) + double recipNorm; + double s0, s1, s2, s3; + double qDot1, qDot2, qDot3, qDot4; + + // Rate of change of quaternion from gyroscope + qDot1 = 0.5 * (-q1*gx - q2*gy - q3*gz); + qDot2 = 0.5 * ( q0*gx + q2*gz - q3*gy); + qDot3 = 0.5 * ( q0*gy - q1*gz + q3*gx); + qDot4 = 0.5 * ( q0*gz + q1*gy - q2*gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) { + + // Normalise accelerometer measurement + recipNorm = 1.0 / sqrt(ax*ax + ay*ay + az*az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Gradient decent algorithm corrective step + s0 = 4.0*q0*q2*q2 + 2.0*q2*ax + 4.0*q0*q1*q1 - 2.0*q1*ay; + s1 = 4.0*q1*q3*q3 - 2.0*q3*ax + 4.0*q0*q0*q1 - 2.0*q0*ay - 4.0*q1 + 8.0*q1*q1*q1 + 8.0*q1*q2*q2 + 4.0*q1*az; + s2 = 4.0*q0*q0*q2 + 2.0*q0*ax + 4.0*q2*q3*q3 - 2.0*q3*ay - 4.0*q2 + 8.0*q2*q1*q1 + 8.0*q2*q2*q2 + 4.0*q2*az; + s3 = 4.0*q1*q1*q3 - 2.0*q1*ax + 4.0*q2*q2*q3 - 2.0*q2*ay; + + // Normalise step magnitude + recipNorm = 1.0 / sqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3); + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * dt; + q1 += qDot2 * dt; + q2 += qDot3 * dt; + q3 += qDot4 * dt; + + // Normalise quaternion + recipNorm = 1.0 / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + q->w = q0; + q->x = q1; + q->y = q2; + q->z = q3; +} diff --git a/mainloop/geometry.h b/mainloop/geometry.h index 9886f50..2dbaf54 100644 --- a/mainloop/geometry.h +++ b/mainloop/geometry.h @@ -37,4 +37,6 @@ void eulerangle_toradians(eulerangle_t* a); void quaternion_init(quaternion_t* q); void quaternion_toeuler(quaternion_t* q, eulerangle_t* a); +void quaternion_madgwick(quaternion_t* q, imuvec_t* m, double dt); + #endif diff --git a/mainloop/madgwick.c b/mainloop/madgwick.c deleted file mode 100644 index cb8066b..0000000 --- a/mainloop/madgwick.c +++ /dev/null @@ -1,83 +0,0 @@ -/* - * Copyright 2022 Oleg Borodin - */ - - -#include -#include - -#include - -void madgwick(double dt, quaternion_t* q, imuvec_t* m) { - - double q0 = q->w; - double q1 = q->x; - double q2 = q->y; - double q3 = q->z; // quaternion of sensor frame relative to auxiliary frame - - double gx = m->gx; - double gy = m->gy; - double gz = m->gz; - - double ax = m->ax; - double ay = m->ay; - double az = m->az; - - double beta = 0.1f; // 2 * proportional gain (Kp) - double recipNorm; - double s0, s1, s2, s3; - double qDot1, qDot2, qDot3, qDot4; - - // Rate of change of quaternion from gyroscope - qDot1 = 0.5 * (-q1*gx - q2*gy - q3*gz); - qDot2 = 0.5 * ( q0*gx + q2*gz - q3*gy); - qDot3 = 0.5 * ( q0*gy - q1*gz + q3*gx); - qDot4 = 0.5 * ( q0*gz + q1*gy - q2*gx); - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) { - - // Normalise accelerometer measurement - recipNorm = 1.0 / sqrt(ax*ax + ay*ay + az*az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Gradient decent algorithm corrective step - s0 = 4.0*q0*q2*q2 + 2.0*q2*ax + 4.0*q0*q1*q1 - 2.0*q1*ay; - s1 = 4.0*q1*q3*q3 - 2.0*q3*ax + 4.0*q0*q0*q1 - 2.0*q0*ay - 4.0*q1 + 8.0*q1*q1*q1 + 8.0*q1*q2*q2 + 4.0*q1*az; - s2 = 4.0*q0*q0*q2 + 2.0*q0*ax + 4.0*q2*q3*q3 - 2.0*q3*ay - 4.0*q2 + 8.0*q2*q1*q1 + 8.0*q2*q2*q2 + 4.0*q2*az; - s3 = 4.0*q1*q1*q3 - 2.0*q1*ax + 4.0*q2*q2*q3 - 2.0*q2*ay; - - // Normalise step magnitude - recipNorm = 1.0 / sqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3); - s0 *= recipNorm; - s1 *= recipNorm; - s2 *= recipNorm; - s3 *= recipNorm; - - // Apply feedback step - qDot1 -= beta * s0; - qDot2 -= beta * s1; - qDot3 -= beta * s2; - qDot4 -= beta * s3; - } - - // Integrate rate of change of quaternion to yield quaternion - q0 += qDot1 * dt; - q1 += qDot2 * dt; - q2 += qDot3 * dt; - q3 += qDot4 * dt; - - // Normalise quaternion - recipNorm = 1.0 / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - q->w = q0; - q->x = q1; - q->y = q2; - q->z = q3; -} diff --git a/mainloop/madgwick.h b/mainloop/madgwick.h deleted file mode 100644 index 8f30f4d..0000000 --- a/mainloop/madgwick.h +++ /dev/null @@ -1,13 +0,0 @@ -/* - * Copyright 2022 Oleg Borodin - */ - - -#ifndef MADGWIC_H_QWERTY -#define MADGWIC_H_QWERTY - -#include - -void madgwick(double dt, quaternion_t* q, imuvec_t* m); - -#endif diff --git a/mainloop/main.c b/mainloop/main.c index 0d25a87..233e827 100644 --- a/mainloop/main.c +++ b/mainloop/main.c @@ -23,7 +23,6 @@ #include #include #include -#include #include #include #include @@ -257,8 +256,6 @@ void uav_loop(uav_t* uav) { double dt = systimer_apply(&(uav->systimer), g_sys_tick_counter); - //printf("%0.8f ival #1 = %10.5f %10.5f %10.5f\r\n", dt, uav->ival.gx, uav->ival.gy, uav->ival.gz); - uav->ival.gx = lpf3_apply(&(uav->lpfgx), uav->ival.gx, dt); uav->ival.gy = lpf3_apply(&(uav->lpfgy), uav->ival.gy, dt); uav->ival.gz = lpf3_apply(&(uav->lpfgz), uav->ival.gz, dt); @@ -267,13 +264,10 @@ void uav_loop(uav_t* uav) { uav->ival.ay = lpf3_apply(&(uav->lpfay), uav->ival.ay, dt); uav->ival.az = lpf3_apply(&(uav->lpfaz), uav->ival.az, dt); - //printf("%0.8f ival #2 = %10.5f %10.5f %10.5f\r\n", dt, uav->ival.gx, uav->ival.gy, uav->ival.gz); - - madgwick(dt, &(uav->q), &(uav->ival)); + quaternion_madgwick(&(uav->q), &(uav->ival), dt); quaternion_toeuler(&(uav->q), &(uav->a)); - eulerangle_todegress(&(uav->a)); - //printf("%.8f 0x%8.3f 0x%8.3f 0x%8.3f\r\n", dt, uav->a.x, uav->a.y, uav->a.z); + eulerangle_todegress(&(uav->a)); mixer_apply(&(uav->mix));