fix madgwick params config

Addresses #653.

analysis/statistics/bd31828fc5af3a17a056eb19063d7349ae2bd9a1.txt

@@ -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) 
+
+
+
+

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

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

applications/newton/llvm-ir/c-files/MadgwickAHRSfix.h

@@ -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);

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: