ziggi/stm32uav
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

madgwick() moved to geometry.c

Browse Source
This commit is contained in:
Oleg Borodin
2022-09-27 12:43:59 +02:00
parent 38dfa0c3ef
commit 8e211fd8fb
6 changed files with 79 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
mainloop/Makefile
View File

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

75
mainloop/geometry.c
View File

@@ -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;
}

2
mainloop/geometry.h
View File

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

83
mainloop/madgwick.c
View File

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

13
mainloop/madgwick.h
View File

@@ -1,13 +0,0 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#ifndef MADGWIC_H_QWERTY
#define MADGWIC_H_QWERTY
#include <geometry.h>
void madgwick(double dt, quaternion_t* q, imuvec_t* m);
#endif

10
mainloop/main.c
View File

@@ -23,7 +23,6 @@
#include <usartu.h>
#include <mpu6050.h>
#include <geometry.h>
#include <madgwick.h>
#include <pidcont.h>
#include <filter.h>
#include <mixer.h>
@@ -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));