From 0e97c5693eba0399a0b980aa080ca2382e025ac3 Mon Sep 17 00:00:00 2001 From: Pei Mu Date: Mon, 5 Jun 2023 16:45:12 +0100 Subject: [PATCH] fix madgwick params config Addresses #653. --- ...31828fc5af3a17a056eb19063d7349ae2bd9a1.txt | 46 +++++++++++++++++++ .../newton/llvm-ir/c-files/MadgwickAHRS.c | 4 +- .../newton/llvm-ir/c-files/MadgwickAHRSfix.c | 15 +++--- .../newton/llvm-ir/c-files/MadgwickAHRSfix.h | 3 ++ applications/newton/llvm-ir/test_madgwick.c | 30 ++++++------ 5 files changed, 75 insertions(+), 23 deletions(-) create mode 100644 analysis/statistics/bd31828fc5af3a17a056eb19063d7349ae2bd9a1.txt diff --git a/analysis/statistics/bd31828fc5af3a17a056eb19063d7349ae2bd9a1.txt b/analysis/statistics/bd31828fc5af3a17a056eb19063d7349ae2bd9a1.txt new file mode 100644 index 000000000..d457214a8 --- /dev/null +++ b/analysis/statistics/bd31828fc5af3a17a056eb19063d7349ae2bd9a1.txt @@ -0,0 +1,46 @@ + +changeset: 1497:bd31828fc5af3a17a056eb19063d7349ae2bd9a1 +char kNewtonVersion[] = "0.3-alpha-1497 (bd31828fc5af3a17a056eb19063d7349ae2bd9a1) (build 05-31-2023-16:03-pei@pei-G5-5500-Linux-5.19.0-42-generic-x86_64)"; +\n./src/noisy/noisy-linux-EN -O0 applications/noisy/helloWorld.n -s +\n./src/newton/newton-linux-EN -v 0 -eP applications/newton/invariants/ViolinWithTemperatureDependence-pigroups.nt + +Informational Report: +--------------------- +Invariant "ViolinWithTemperatureDependenceForPiGroups" has 2 unique kernels, each with 2 column(s)... + + Kernel 0 is a valid kernel: + + 1 1 + -0.5 -0 + 1 0 + 0.5 0 + 0 -1 + -0 -1 + + + The ordering of parameters is: P1 P0 P3 P2 P4 P5 + + Pi group 0, Pi 0 is: P0^(-0.5) P1^( 1) P2^(0.5) P3^( 1) P4^( 0) P5^(-0) + + Pi group 0, Pi 1 is: P0^(-0) P1^( 1) P2^( 0) P3^( 0) P4^(-1) P5^(-1) + + + Kernel 1 is a valid kernel: + + 1 0 + -0.5 1 + 1 -2 + 0.5 -1 + -0 -2 + 0 -2 + + + The ordering of parameters is: P1 P0 P3 P2 P4 P5 + + Pi group 1, Pi 0 is: P0^(-0.5) P1^( 1) P2^(0.5) P3^( 1) P4^(-0) P5^( 0) + + Pi group 1, Pi 1 is: P0^( 1) P1^( 0) P2^(-1) P3^(-2) P4^(-2) P5^(-2) + + + + diff --git a/applications/newton/llvm-ir/c-files/MadgwickAHRS.c b/applications/newton/llvm-ir/c-files/MadgwickAHRS.c index ad95f2711..a36a5b99f 100644 --- a/applications/newton/llvm-ir/c-files/MadgwickAHRS.c +++ b/applications/newton/llvm-ir/c-files/MadgwickAHRS.c @@ -37,7 +37,7 @@ // Variable definitions volatile float beta = betaDef; // 2 * proportional gain (Kp) -volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame +volatile float q0 = 0.64306622f, q1 = 0.02828862f, q2 = -0.00567953f, q3 = -0.76526684f; // quaternion of sensor frame relative to auxiliary frame //--------------------------------------------------------------------------------------------------- // Function declarations @@ -81,7 +81,7 @@ void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; - az *= recipNorm; + az *= recipNorm; // Normalise magnetometer measurement recipNorm = invSqrt(mx * mx + my * my + mz * mz); diff --git a/applications/newton/llvm-ir/c-files/MadgwickAHRSfix.c b/applications/newton/llvm-ir/c-files/MadgwickAHRSfix.c index 638833e6e..49d18ec35 100644 --- a/applications/newton/llvm-ir/c-files/MadgwickAHRSfix.c +++ b/applications/newton/llvm-ir/c-files/MadgwickAHRSfix.c @@ -20,8 +20,9 @@ typedef int32_t bmx055xMagneto; typedef int32_t bmx055yMagneto; typedef int32_t bmx055zMagneto; -bmx055xAcceleration beta = (uint8_t)(betaDef*FRAC_BASE); //0.1f // 2 * proportional gain (Kp) -bmx055xAcceleration q0 = FRAC_BASE, q1 = 0x0, q2 = 0x0, q3 = 0x0; // quaternion of sensor frame relative to auxiliary frame +volatile bmx055xAcceleration beta = (uint8_t)(betaDef*FRAC_BASE); //0.1f // 2 * proportional gain (Kp) +volatile bmx055xAcceleration q0 = 0.64306622f*FRAC_BASE, q1 = 0.02828862f*FRAC_BASE, + q2 = -0.00567953f*FRAC_BASE, q3 = -0.76526684f*FRAC_BASE; // quaternion of sensor frame relative to auxiliary frame // m=-7 1/Yest=0.0858 Yest=11.3120 Yest_hex=B50 // m=-6 1/Yest=0.1248 Yest=8.0000 Yest_hex=800 @@ -54,8 +55,6 @@ bmx055xAcceleration q0 = FRAC_BASE, q1 = 0x0, q2 = 0x0, q3 = 0x0; // quaternion // m=21 1/Yest=1448.0000 Yest=0.0000 Yest_hex=0 // m=22 1/Yest=2048.0000 Yest=0.0000 Yest_hex=0 -#define MULFIX(_op1, _op2) (((int64_t)_op1*_op2)/FRAC_BASE) - int32_t mulfix(int32_t x, int32_t y) { @@ -161,7 +160,7 @@ MadgwickAHRSupdate(bmx055xAngularRate gx, bmx055yAngularRate gy, bmx055zAngularR recipNorm = sqrt_rsqrt(mulfix(ax, ax) + mulfix(ay, ay) + mulfix(az, az), true); ax = mulfix(ax, recipNorm); ay = mulfix(ay, recipNorm); - az = mulfix(az, recipNorm); + az = mulfix(az, recipNorm); // Normalise magnetometer measurement recipNorm = sqrt_rsqrt(mulfix(mx, mx) + mulfix(my, my) + mulfix(mz, mz), true); @@ -312,7 +311,11 @@ MadgwickAHRSupdate(bmx055xAngularRate gx, bmx055yAngularRate gy, bmx055zAngularR q2 = mulfix(q2, recipNorm); q3 = mulfix(q3, recipNorm); -// printf("FIX: q0 = %d\n", q0); +// printf("FIX: q0 = %d.%04d, q1 = %d.%04d, q2 = %d.%04d\n", +// DISPLAY_INT(q0), DISPLAY_FRAC(q0), +// DISPLAY_INT(q1), DISPLAY_FRAC(q1), +// DISPLAY_INT(q2), DISPLAY_FRAC(q2)); +// printf("q0=%d, q1=%d, q2=%d\n", q0, q1, q2); // /* 2nd iter normalization */ // recipNorm = sqrt_rsqrt(mulfix(q0, q0) + mulfix(q1, q1) + mulfix(q2, q2) + mulfix(q3, q3), true); diff --git a/applications/newton/llvm-ir/c-files/MadgwickAHRSfix.h b/applications/newton/llvm-ir/c-files/MadgwickAHRSfix.h index c147e0233..2859d311a 100644 --- a/applications/newton/llvm-ir/c-files/MadgwickAHRSfix.h +++ b/applications/newton/llvm-ir/c-files/MadgwickAHRSfix.h @@ -20,6 +20,9 @@ #define FRAC_Q 8 #define FRAC_BASE (1<>FRAC_Q) + (((uint32_t)_x)>>(sizeof(int32_t)*FRAC_Q-1))) +#define DISPLAY_FRAC(_y) (((FRAC_BASE-1)&_y) * (10000/FRAC_BASE)) +#define FORMAT_FIXP "%s%d.%04d" void MadgwickAHRSupdate(int32_t gx, int32_t gy, int32_t gz, int32_t ax, int32_t ay, int32_t az, int32_t mx, int32_t my, int32_t mz); void MadgwickAHRSupdateIMU(int32_t gx, int32_t gy, int32_t gz, int32_t ax, int32_t ay, int32_t az); diff --git a/applications/newton/llvm-ir/test_madgwick.c b/applications/newton/llvm-ir/test_madgwick.c index c6121330d..d28980267 100644 --- a/applications/newton/llvm-ir/test_madgwick.c +++ b/applications/newton/llvm-ir/test_madgwick.c @@ -124,65 +124,65 @@ int main() { break; case 1: #if defined(INT_DATA_TYPE) - mag_x[row-2] = atoi(value); + mag_x[row-2] = atoi(value)*FRAC_BASE; #elif defined(FP_DATA_TYPE) mag_x[row-2] = atof(value); #endif break; case 2: #if defined(INT_DATA_TYPE) - mag_y[row-2] = atoi(value); + mag_y[row-2] = atoi(value)*FRAC_BASE; #elif defined(FP_DATA_TYPE) mag_y[row-2] = atof(value); #endif break; case 3: #if defined(INT_DATA_TYPE) - mag_z[row-2] = atoi(value); + mag_z[row-2] = atoi(value)*FRAC_BASE; #elif defined(FP_DATA_TYPE) mag_z[row-2] = atof(value); #endif break; case 4: #if defined(INT_DATA_TYPE) - gyr_x[row-2] = atoi(value); + gyr_x[row-2] = atoi(value)*FRAC_BASE*61; #elif defined(FP_DATA_TYPE) - gyr_x[row-2] = atof(value); + gyr_x[row-2] = atof(value)*61; #endif break; case 5: #if defined(INT_DATA_TYPE) - gyr_y[row-2] = atoi(value); + gyr_y[row-2] = atoi(value)*FRAC_BASE*61; #elif defined(FP_DATA_TYPE) - gyr_y[row-2] = atof(value); + gyr_y[row-2] = atof(value)*61; #endif break; case 6: #if defined(INT_DATA_TYPE) - gyr_z[row-2] = atoi(value); + gyr_z[row-2] = atoi(value)*FRAC_BASE*61; #elif defined(FP_DATA_TYPE) - gyr_z[row-2] = atof(value); + gyr_z[row-2] = atof(value)*61; #endif break; case 7: #if defined(INT_DATA_TYPE) - acc_x[row-2] = atoi(value); + acc_x[row-2] = atoi(value)*FRAC_BASE*2; #elif defined(FP_DATA_TYPE) - acc_x[row-2] = atof(value); + acc_x[row-2] = atof(value)*2; #endif break; case 8: #if defined(INT_DATA_TYPE) - acc_y[row-2] = atoi(value); + acc_y[row-2] = atoi(value)*FRAC_BASE*2; #elif defined(FP_DATA_TYPE) - acc_y[row-2] = atof(value); + acc_y[row-2] = atof(value)*2; #endif break; case 9: #if defined(INT_DATA_TYPE) - acc_z[row-2] = atoi(value); + acc_z[row-2] = atoi(value)*FRAC_BASE*2; #elif defined(FP_DATA_TYPE) - acc_z[row-2] = atof(value); + acc_z[row-2] = atof(value)*2; #endif break; default: