Diff: main.cpp

Revision:

23:226f18dd30a5

Parent:

21:aab1ac72095b

--- a/main.cpp	Mon Jan 27 09:24:35 2020 +0000
+++ b/main.cpp	Thu Jan 30 14:14:50 2020 +0000
@@ -22,13 +22,21 @@
 Matrix<float, 9, 1> b;
 Matrix<float, 8, 8> I;
 
-Matrix<float, 7, 1> gyro_x;
-Matrix<float, 7, 1> gyro_y;
-Matrix<float, 7, 1> gyro_z;
-Matrix<float, 7, 1> accel_x;
-Matrix<float, 7, 1> accel_y;
-Matrix<float, 7, 1> magnet_x;
-Matrix<float, 7, 1> magnet_y;
+// Matrix<float, 7, 1> gyro_x;
+// Matrix<float, 7, 1> gyro_y;
+// Matrix<float, 7, 1> gyro_z;
+// Matrix<float, 7, 1> accel_x;
+// Matrix<float, 7, 1> accel_y;
+// Matrix<float, 7, 1> magnet_x;
+// Matrix<float, 7, 1> magnet_y;
+
+Matrix<float, 14, 1> gyro_x;
+Matrix<float, 14, 1> gyro_y;
+Matrix<float, 14, 1> gyro_z;
+Matrix<float, 14, 1> accel_x;
+Matrix<float, 14, 1> accel_y;
+Matrix<float, 14, 1> magnet_x;
+Matrix<float, 14, 1> magnet_y;
 
 int main()
 {
@@ -81,19 +89,27 @@
     I.setIdentity();
     // I = I.transpose();
     
-    gyro_x   <<  0.1,  0.5, -0.2,  0.2,  0.3,  0.2, -0.7;
-    gyro_y   << -0.1, -0.3,  0.1,  0.1,  0.1, -0.1, -0.9;
-    gyro_z   <<  0.5,  0.4, -0.3,  0.2,  0.8, -0.6,  0.2;
-    accel_x  <<  0.3, -0.1,  0.7, -0.3, -0.9,  0.1,  0.2;
-    accel_y  <<  0.1,  0.5, -0.4, -0.2, -0.4,  0.5,  0.8;
-    magnet_x <<  0.1, -0.2, -0.8,  0.2,  0.6,  0.4, -0.4;
-    magnet_y <<  0.6, -0.4,  0.4,  0.7,  0.7,  0.5,  0.3;
+    // gyro_x   <<  0.1,  0.5, -0.2,  0.2,  0.3,  0.2, -0.7;
+    // gyro_y   << -0.1, -0.3,  0.1,  0.1,  0.1, -0.1, -0.9;
+    // gyro_z   <<  0.5,  0.4, -0.3,  0.2,  0.8, -0.6,  0.2;
+    // accel_x  <<  0.3, -0.1,  0.7, -0.3, -0.9,  0.1,  0.2;
+    // accel_y  <<  0.1,  0.5, -0.4, -0.2, -0.4,  0.5,  0.8;
+    // magnet_x <<  0.1, -0.2, -0.8,  0.2,  0.6,  0.4, -0.4;
+    // magnet_y <<  0.6, -0.4,  0.4,  0.7,  0.7,  0.5,  0.3;
+    
+    gyro_x   <<  0.1f,  0.5f, -0.2f,  0.2f,  0.3f,  0.2f, -0.7f, 0.5f,  0.4f, -0.3f,  0.2f,  0.8f, -0.6f,  0.2f;
+    gyro_y   << -0.1f, -0.3f,  0.1f,  0.1f,  0.1f, -0.1f, -0.9f, 0.1f,  0.5f, -0.4f, -0.2f, -0.4f,  0.5f,  0.8f;
+    gyro_z   <<  0.5f,  0.4f, -0.3f,  0.2f,  0.8f, -0.6f,  0.2f,-0.1f, -0.3f,  0.1f,  0.1f,  0.1f, -0.1f, -0.9f;
+    accel_x  <<  0.3f, -0.1f,  0.7f, -0.3f, -0.9f,  0.1f,  0.2f, 0.1f,  0.5f, -0.2f,  0.2f,  0.3f,  0.2f, -0.7f;
+    accel_y  <<  0.1f,  0.5f, -0.4f, -0.2f, -0.4f,  0.5f,  0.8f, 0.1f, -0.2f, -0.8f,  0.2f,  0.6f,  0.4f, -0.4f;
+    magnet_x <<  0.1f, -0.2f, -0.8f,  0.2f,  0.6f,  0.4f, -0.4f, 0.6f, -0.4f,  0.4f,  0.7f,  0.7f,  0.5f,  0.3f;
+    magnet_y <<  0.6f, -0.4f,  0.4f,  0.7f,  0.7f,  0.5f,  0.3f, 0.3f, -0.1f,  0.7f, -0.3f, -0.9f,  0.1f,  0.2f;
         
     pc.printf("\r\n");
         
     while(1) {
         
-        Matrix<float, 9, 1> x = A.inverse() * b;
+        // Matrix<float, 9, 1> x = A.inverse() * b;
         
         // float temp0 = (float)(rand()%10000-4999)*0.00001f;
         // float temp1 = (float)(rand()%10000-4999)*0.00001f;
@@ -105,17 +121,17 @@
         dt = timer.read();
         timer.reset();
         
-        if( i < 7 ) {
+        if( i < 14 ) {
             /* ekf_rp */
             ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i));
-            pc.printf("%i; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; \r\n", i, ekf_rp.get_est_state(0), ekf_rp.get_est_state(1), ekf_rp.get_est_state(2), ekf_rp.get_est_state(3), ekf_rp.get_est_state(4), ekf_rp.get_est_state(5), dt);
-            // pc.printf("%.7f; \r\n", dt);
+            // pc.printf("%i; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; \r\n", i, ekf_rp.get_est_state(0), ekf_rp.get_est_state(1), ekf_rp.get_est_state(2), ekf_rp.get_est_state(3), ekf_rp.get_est_state(4), ekf_rp.get_est_state(5), dt);
+            pc.printf("%.7f; \r\n", dt);
             /* ekf_rpy */
             // ekf_rpy.update(gyro_x(i), gyro_y(i), gyro_z(i), accel_x(i), accel_y(i), magnet_x(i), magnet_y(i));
             // pc.printf("%i; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f %.7f; %.7f; \r\n", i, ekf_rpy.get_est_state(0), ekf_rpy.get_est_state(1), ekf_rpy.get_est_state(2), ekf_rpy.get_est_state(3), ekf_rpy.get_est_state(4), ekf_rpy.get_est_state(5), ekf_rpy.get_est_state(6), ekf_rpy.get_est_state(7), dt);
             // pc.printf("%.7f; \r\n", dt);
         }
         i++;        
-        wait_us(1000000);
+        // wait_us(1000000);
     }
 }