Diff: main.cpp

Revision:

1:39129aaf5c80

Parent:

0:384bc42ad0cf

Child:

2:cde1397d3fc4

Child:

3:02877f5ca29e

diff -r 384bc42ad0cf -r 39129aaf5c80 main.cpp
--- a/main.cpp	Wed Aug 15 22:20:07 2012 +0000
+++ b/main.cpp	Wed Mar 08 09:03:52 2017 +0000
@@ -1,14 +1,20 @@
 #include "mbed.h"
 #include "kalman.c"
-#include "BMA180.h"
-#include "ITG3200.h"
+#include "x_nucleo_iks01a1.h"
+#include <math.h>
 
 #define PI             3.1415926535897932384626433832795
 #define Rad2Dree       57.295779513082320876798154814105
 
-Serial pc(USBTX, USBRX);
+/* Instantiate the expansion board */
+static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
 
-I2C I2CBus(p28, p27);
+/* 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;
+
+Serial pc(USBTX, USBRX);
 
 Timer GlobalTime;
 Timer ProgramTimer;
@@ -17,34 +23,18 @@
 double angle[3];
 unsigned long timer;
 long loopStartTime;
+int32_t axesAcc[3];
+int32_t axesGyro[3];
 
-BMA180 Acc(I2CBus, GlobalTime);
-ITG3200 Gyro(I2CBus, GlobalTime);
-kalman filter_pitch; 
 kalman filter_roll;
+kalman filter_pitch;
+kalman filter_yaw; 
     
 int main() {
-    int count = 0;
+//    int count = 0;
     pc.baud(115200);
   
-    I2CBus.frequency(400000);
-    GlobalTime.start();
-
-    Acc.Init();
-    wait_ms(500);
-    
-    //User Calibration
-    short Raw1g[3]= {0, 0, 0};
-    Acc.userCalibration(Raw1g);
-    
-    //0.5s Calibration
-    Acc.Calibrate(500, Raw1g);
-  
-    Gyro.Init();
-    wait_ms(500);
-    
-    //0.5s Calibration
-    Gyro.Calibrate(500);
+//    GlobalTime.start();
 
     // Parameters ( R_angle, Q_gyro, Q_angle ) 
     kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
@@ -58,25 +48,28 @@
     while(1) {
     
         //Aquire new values for the Gyro and Acc
-        Acc.Update();
-        Gyro.Update();
+          accelerometer->Get_X_Axes(axesAcc);
+          gyroscope->Get_G_Axes(axesGyro);
+
         
         //Calcuate the resulting vector R from the 3 acc axes
-        R = sqrt(std::pow(Acc.Acc[0] , 2) + std::pow(Acc.Acc[1] , 2) + std::pow(Acc.Acc[2] , 2));
+        R = sqrt(std::pow((float)axesAcc[0] , 2) + std::pow((float)axesAcc[1] , 2) + std::pow((float)axesAcc[2] , 2));
    
-        kalman_predict(&filter_pitch, Gyro.Rate[0],  (ProgramTimer.read_us() - timer)); 
-        kalman_update(&filter_pitch, acos(Acc.Acc[0]/R));
-        kalman_predict(&filter_roll, Gyro.Rate[1],  (ProgramTimer.read_us() - timer)); 
-        kalman_update(&filter_roll, acos(Acc.Acc[1]/R));
+        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));
         
-        angle[0] = kalman_get_angle(&filter_pitch);
-        angle[1] = kalman_get_angle(&filter_roll);
         
+        angle[0] = kalman_get_angle(&filter_roll);
+        angle[1] = kalman_get_angle(&filter_pitch);
+        angle[2] = kalman_get_angle(&filter_yaw);
+                
         timer = ProgramTimer.read_us();
         
-        //count++;
-        //if(count == 10000){pc.printf("Program time: %i\n\r", ProgramTimer.read_us() - loopStartTime);loopStartTime = ProgramTimer.read_us(); count = 0;}
-        pc.printf("Pitch Angle X: %.6f   Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]);
-       
+        pc.printf("Roll Angle X: %.6f   Pitch Angle Y: %.6f Yaw Angle Z: %.6f \n\r", Rad2Dree * angle[0], Rad2Dree * angle[1], Rad2Dree * angle[2]);
+        
     }
 }