Diff: main.cpp

Revision:

11:3f0b35a0855c

Parent:

10:28fa811afbfb

Child:

12:5638ddcd8477

--- a/main.cpp	Fri Jun 17 14:30:55 2016 +0000
+++ b/main.cpp	Fri Jun 17 14:41:55 2016 +0000
@@ -48,7 +48,8 @@
 //---------------------------------------------------------------------------------------------------
 // AHRS algorithm update
 
-void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
+{
     float recipNorm;
     float s0, s1, s2, s3;
     float qDot1, qDot2, qDot3, qDot4;
@@ -74,7 +75,7 @@
         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);
@@ -147,7 +148,8 @@
 //---------------------------------------------------------------------------------------------------
 // IMU algorithm update
 
-void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
+void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az)
+{
     float recipNorm;
     float s0, s1, s2, s3;
     float qDot1, qDot2, qDot3, qDot4;
@@ -166,7 +168,7 @@
         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
         ax *= recipNorm;
         ay *= recipNorm;
-        az *= recipNorm;   
+        az *= recipNorm;
 
         // Auxiliary variables to avoid repeated arithmetic
         _2q0 = 2.0f * q0;
@@ -219,7 +221,8 @@
 // Fast inverse square-root
 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
 
-float invSqrt(float x) {
+float invSqrt(float x)
+{
     float halfx = 0.5f * x;
     float y = x;
     long i = *(long*)&y;
@@ -241,18 +244,52 @@
 float x,y,z,gxOfs,gyOfs,gzOfs;
 // Calibration wait
 
-void init(void){
-    
+void resetRot(void)
+{
+
+    gxOfs = 0;
+    gyOfs = 0;
+    gzOfs = 0;
+
+    imu[0]->deselect();
+    imu[1]->deselect();
+
+    imu[0]->select();
+
+    for(int i=0; i<1000; i++) {
+
+        imu[0]->read_all();
+
+        gxOfs += imu[0]->gyroscope_data[0];
+        gyOfs += imu[0]->gyroscope_data[1];
+        gzOfs += imu[0]->gyroscope_data[2];
+
+        wait_us(1000000.0f/sampleFreq);
+    }
+
+    gxOfs /= 1000;
+    gyOfs /= 1000;
+    gzOfs /= 1000;
+
+    q0 = 1.0f;
+    q1 = 0.0f;
+    q2 = 0.0f;
+    q3 = 0.0f;
+}
+
+void init(void)
+{
+
     pc.baud(921600);
-    
+
     imu[0] = new mpu9250_spi(spi, p8);
     imu[1] = new mpu9250_spi(spi, p9);
-    
-    for(int i=0; i<12; i++)     
+
+    for(int i=0; i<12; i++)
         kf[i] = new KalmanFilter(1e-3, 0.001);
-        
+
     for(int i=0; i<2; i++) {
-    
+
         imu[0]->deselect();
         imu[1]->deselect();
         imu[i]->select();
@@ -268,40 +305,16 @@
         imu[i]->AK8963_calib_Magnetometer();
         wait(0.1);
     }
-    
-    gxOfs = 0;
-    gyOfs = 0;
-    gzOfs = 0;
-    
-    imu[0]->deselect();
-    imu[1]->deselect();
-    
-    imu[0]->select();
-    
-    for(int i=0; i<1000; i++) {
-        
-        imu[0]->read_all();
-        
-        gxOfs += imu[0]->gyroscope_data[0];
-        gyOfs += imu[0]->gyroscope_data[1];
-        gzOfs += imu[0]->gyroscope_data[2];
-        
-        wait_us(1000000.0f/sampleFreq);
-    }
-    
-    gxOfs /= 1000;
-    gyOfs /= 1000;
-    gzOfs /= 1000;
-    
+    resetRot();
 }
 
 void eventFunc(void)
 {
     for(int i=0; i<1; i++) {
-        
+
         imu[0]->deselect();
         imu[1]->deselect();
-        
+
         imu[i]->select();
         imu[i]->read_all();
 
@@ -316,35 +329,44 @@
             imu[i]->Magnetometer[1],
             imu[i]->Magnetometer[2]
         );
-        
-        printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f\n", q0,q1,q2,q3, q0,q1,q2,q3, q0,q1,q2,q3, q0,q1,q2,q3 );
-    }  
+
+        printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f\n", 
+        q0,q1,q2,q3, 
+        q0,q1,q2,q3, 
+        q0,q1,q2,q3, 
+        q0,q1,q2,q3 );
+    }
 }
 
 int main()
 {
     init();
-    
-    ticker.attach_us(eventFunc, 1000000.0f/sampleFreq); // 512Hz
-    
+
+    ticker.attach_us(eventFunc, 1000000.0f/sampleFreq);
+
     while(1) {
-        
-        //name.readable();
-            /*
-            imu[i]->read_all();
-            printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ",
-                   imu[i]->Temperature,
-                   imu[i]->gyroscope_data[0],
-                   imu[i]->gyroscope_data[1],
-                   imu[i]->gyroscope_data[2],
-                   imu[i]->accelerometer_data[0],
-                   imu[i]->accelerometer_data[1],
-                   imu[i]->accelerometer_data[2],
-                   imu[i]->Magnetometer[0],
-                   imu[i]->Magnetometer[1],
-                   imu[i]->Magnetometer[2]
-                  );*/
-            //myled = 0;
-            //wait(0.5);
+
+        if(pc.readable())
+            if(pc.getc() == 'r') {
+                ticker.detach();
+                resetRot();
+                ticker.attach_us(eventFunc, 1000000.0f/sampleFreq);
+            }
+        /*
+        imu[i]->read_all();
+        printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ",
+               imu[i]->Temperature,
+               imu[i]->gyroscope_data[0],
+               imu[i]->gyroscope_data[1],
+               imu[i]->gyroscope_data[2],
+               imu[i]->accelerometer_data[0],
+               imu[i]->accelerometer_data[1],
+               imu[i]->accelerometer_data[2],
+               imu[i]->Magnetometer[0],
+               imu[i]->Magnetometer[1],
+               imu[i]->Magnetometer[2]
+              );*/
+        //myled = 0;
+        //wait(0.5);
     }
 }