m5/M5Stack
0
0
Fork 0
mirror of https://github.com/m5stack/M5Stack.git synced 2026-05-20 10:06:46 -07:00
Code Issues Packages Projects Releases Wiki Activity

Prevent double declaration

Browse Source
This commit is contained in:
GoT
2022-03-14 12:23:18 +01:00
committed by GitHub
parent f85bf01def
commit c696151181
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/utility/quaternionFilters.cpp
+5 -5
View File
@@ -32,7 +32,7 @@ static float GyroMeasError = PI * (40.0f / 180.0f);
// the faster the solution converges, usually at the expense of accuracy.
// In any case, this is the free parameter in the Madgwick filtering and
// fusion scheme.
static float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // Compute beta
static float gyro_beta = sqrt(3.0f / 4.0f) * GyroMeasError; // Compute beta
// Compute zeta, the other free parameter in the Madgwick scheme usually
// set to a small or zero value
// static float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;
@@ -120,10 +120,10 @@ void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy,
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - gyro_beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - gyro_beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - gyro_beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - gyro_beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * deltat;