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];