Revision 3:02877f5ca29e, committed 2017-03-09

Comitter:

Carminio

Date:

Thu Mar 09 13:14:31 2017 +0000

Parent:

1:39129aaf5c80

Commit message:

TestVer for yaw calculation. Not working.

Changed in this revision

--- a/main.cpp	Wed Mar 08 09:03:52 2017 +0000
+++ b/main.cpp	Thu Mar 09 13:14:31 2017 +0000
@@ -12,7 +12,7 @@
 /* Retrieve the composing elements of the expansion board */
 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
-//static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
+static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
 
 Serial pc(USBTX, USBRX);
 
@@ -25,13 +25,13 @@
 long loopStartTime;
 int32_t axesAcc[3];
 int32_t axesGyro[3];
+int32_t axesMag[3];
 
 kalman filter_roll;
 kalman filter_pitch;
 kalman filter_yaw; 
     
 int main() {
-//    int count = 0;
     pc.baud(115200);
   
 //    GlobalTime.start();
@@ -40,32 +40,38 @@
     kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
     kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
     
+    kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+    
     
     ProgramTimer.start();
     loopStartTime = ProgramTimer.read_us();
     timer = loopStartTime;
     
     while(1) {
-    
+//    wait_ms(10);
         //Aquire new values for the Gyro and Acc
           accelerometer->Get_X_Axes(axesAcc);
           gyroscope->Get_G_Axes(axesGyro);
-
+          magnetometer->Get_M_Axes(axesMag);
         
-        //Calcuate the resulting vector R from the 3 acc axes
+        //Calculate the resulting vector R from the 3 acc axes
         R = sqrt(std::pow((float)axesAcc[0] , 2) + std::pow((float)axesAcc[1] , 2) + std::pow((float)axesAcc[2] , 2));
    
         kalman_predict(&filter_roll, axesGyro[0],  (ProgramTimer.read_us() - timer)); 
         kalman_update(&filter_roll, acos(axesAcc[0]/R));
+        
         kalman_predict(&filter_pitch, axesGyro[1],  (ProgramTimer.read_us() - timer)); 
         kalman_update(&filter_pitch, acos(axesAcc[1]/R));
+        
         kalman_predict(&filter_yaw, axesGyro[2],  (ProgramTimer.read_us() - timer)); 
-        kalman_update(&filter_yaw, acos(axesAcc[2]/R));
+        kalman_update(&filter_yaw, acos(axesMag[2]/R));
         
         
         angle[0] = kalman_get_angle(&filter_roll);
         angle[1] = kalman_get_angle(&filter_pitch);
-        angle[2] = kalman_get_angle(&filter_yaw);
+        angle[2] = atan2((double)axesGyro[0],(double)axesGyro[1])*Rad2Dree;
+//        angle[2] = kalman_get_angle(&filter_yaw);
+        atan2((double)axesAcc[0],(double)axesAcc[1])*Rad2Dree; 
                 
         timer = ProgramTimer.read_us();