Diff: main.cpp

Revision:

0:384bc42ad0cf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Aug 15 22:20:07 2012 +0000
@@ -0,0 +1,82 @@
+#include "mbed.h"
+#include "kalman.c"
+#include "BMA180.h"
+#include "ITG3200.h"
+
+#define PI             3.1415926535897932384626433832795
+#define Rad2Dree       57.295779513082320876798154814105
+
+Serial pc(USBTX, USBRX);
+
+I2C I2CBus(p28, p27);
+
+Timer GlobalTime;
+Timer ProgramTimer;
+
+float R;
+double angle[3];
+unsigned long timer;
+long loopStartTime;
+
+BMA180 Acc(I2CBus, GlobalTime);
+ITG3200 Gyro(I2CBus, GlobalTime);
+kalman filter_pitch; 
+kalman filter_roll;
+    
+int main() {
+    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);
+
+    // Parameters ( R_angle, Q_gyro, Q_angle ) 
+    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
+    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
+    
+    
+    ProgramTimer.start();
+    loopStartTime = ProgramTimer.read_us();
+    timer = loopStartTime;
+    
+    while(1) {
+    
+        //Aquire new values for the Gyro and Acc
+        Acc.Update();
+        Gyro.Update();
+        
+        //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));
+   
+        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));
+        
+        angle[0] = kalman_get_angle(&filter_pitch);
+        angle[1] = kalman_get_angle(&filter_roll);
+        
+        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]);
+       
+    }
+}