Skip to content

Commit

Permalink
fix madgwick params config
Browse files Browse the repository at this point in the history 
Addresses #653.
  • Loading branch information
@PeiMu
PeiMu committed Jun 5, 2023
1 parent 9879c5c commit 0e97c56
Show file tree
Hide file tree
Showing 5 changed files with 75 additions and 23 deletions.
46 changes: 46 additions & 0 deletions analysis/statistics/bd31828fc5af3a17a056eb19063d7349ae2bd9a1.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@

changeset: 1497:bd31828fc5af3a17a056eb19063d7349ae2bd9a1
char kNewtonVersion[] = "0.3-alpha-1497 (bd31828fc5af3a17a056eb19063d7349ae2bd9a1) (build 05-31-2023-16:[email protected]_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)




4 changes: 2 additions & 2 deletions applications/newton/llvm-ir/c-files/MadgwickAHRS.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down
15 changes: 9 additions & 6 deletions applications/newton/llvm-ir/c-files/MadgwickAHRSfix.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
3 changes: 3 additions & 0 deletions applications/newton/llvm-ir/c-files/MadgwickAHRSfix.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,9 @@
#define FRAC_Q 8
#define FRAC_BASE (1<<FRAC_Q)
#define DEC2FRAC(_d) ((uint8_t)(_d*FRAC_BASE))
#define DISPLAY_INT(_x) ((_x>>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);
Expand Down
30 changes: 15 additions & 15 deletions applications/newton/llvm-ir/test_madgwick.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down

0 comments on commit 0e97c56

Please sign in to comment.