pima/Trinity-DevPCB-V1.0
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added hemisphere continuity enforcement in the madgwick algorithm

Browse Source
This commit is contained in:
pima committed 2026-09-23 18:31:51 +02:00
1 parent 5e3dd3128b
commit 5daac77a37
4 files changed
+22 -5
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

Core/Src/MadgwickAHRS.c
+17
View File
@@ -148,6 +148,14 @@ void MadgwickAHRS_update(MadgwickAHRS_Filter* filter, float gx, float gy, float
q[2] *= recipNorm;
q[3] *= recipNorm;
// enforce hemisphere continuity (because of the positive and negative doubling of output)
float dot = filter->q[0] * q[0] + filter->q[1] * q[1] + filter->q[2] * q[2] + filter->q[3] * q[3];
if (dot < 0.0f){
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// Update filter state
filter->q[0] = q[0];
filter->q[1] = q[1];
@@ -226,6 +234,15 @@ void MadgwickAHRS_update_IMU(MadgwickAHRS_Filter* filter, float gx, float gy, fl
q[2] *= recipNorm;
q[3] *= recipNorm;
// enforce hemisphere continuity (because of the positive and negative doubling of output)
float dot = filter->q[0] * q[0] + filter->q[1] * q[1] + filter->q[2] * q[2] + filter->q[3] * q[3];
if (dot < 0.0f){
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// Update filter state
filter->q[0] = q[0];
filter->q[1] = q[1];
Debug/Core/Src/MadgwickAHRS.cyclo
+3 -3
View File
@@ -1,4 +1,4 @@
../Core/Src/MadgwickAHRS.c:42:6:MadgwickAHRS_init 1
../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update 7
../Core/Src/MadgwickAHRS.c:161:6:MadgwickAHRS_update_IMU 4
../Core/Src/MadgwickAHRS.c:240:7:invSqrt 1
../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update 8
../Core/Src/MadgwickAHRS.c:169:6:MadgwickAHRS_update_IMU 5
../Core/Src/MadgwickAHRS.c:257:7:invSqrt 1
Debug/Core/Src/MadgwickAHRS.o
BIN
View File
Binary file not shown.
Debug/Core/Src/MadgwickAHRS.su
+2 -2
View File
@@ -1,4 +1,4 @@
../Core/Src/MadgwickAHRS.c:42:6:MadgwickAHRS_init 24 static
../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update 208 static
../Core/Src/MadgwickAHRS.c:161:6:MadgwickAHRS_update_IMU 144 static
../Core/Src/MadgwickAHRS.c:240:7:invSqrt 32 static
../Core/Src/MadgwickAHRS.c:169:6:MadgwickAHRS_update_IMU 152 static
../Core/Src/MadgwickAHRS.c:257:7:invSqrt 32 static