Diff: main.cpp

Revision:

10:2794020bfc0c

Parent:

9:3694ca4b48a7

Child:

11:15181dfe535e

--- a/main.cpp	Mon Dec 08 13:28:17 2014 +0000
+++ b/main.cpp	Fri Dec 12 10:15:10 2014 +0000
@@ -2,6 +2,16 @@
 #include "mbed.h"
 #include "IMU.h"
 #include "KalmanFilter.h"
+#include "kalman.h"
+
+#define M_PI 3.141592
+#define M_PI2 1.570796
+#define dt 0.004
+
+//kalman2
+// Structs for containing filter data
+kalman_data pitch_data;
+kalman_data roll_data;
 
 //DigitalOut myled(LED_RED);
 Serial pc(USBTX, USBRX);
@@ -10,17 +20,22 @@
 
 KalmanFilter kf;
 
-Ticker triger1; //przerwanie filtracji
-Ticker triger2; //przerwanie wysyłania danych
+//Ticker triger1; //przerwanie filtracji
+//Ticker triger2; //przerwanie wysyłania danych
 
 float d[9];
 double D[9];
 float kf_update;
-char buff[40];
+char buff[60];
+float r,katx,katy;
+float pitchk2, rollk2;
 
+/*
 void task1()
 {
     imu.readData(d);
+    r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
+    katx = acos(d[4]/r)*180/M_PI-90;
     //Filtr Buterwortha
     imu.filterData(d, D);
     //sprintf(buff, "%f\n%f\r", d[3], D[3]);
@@ -28,34 +43,66 @@
     //Filtr Kalmana
     kf_update = kf.update(d[2], d[5]);
     //Wyslanie wartosci w protekole szeregowym do Labview (zmienic liczbe danych w labView jest sprintf wysyla wiecej wartosci).
-    sprintf(buff, "%f\n%f\n%f\r", d[5], D[5], (float) kf_update);
+    sprintf(buff, "%f,%f,%f,%f\n\r", katx ,d[5], D[5], (float) kf_update);
 //    sprintf(buff, "%f\n%f\n%f\r", d[6], d[7], d[8]);
 }
 
 void task2()
 {
     pc.printf(buff);
-}
+}*/
 
         
 int main()
 {
-    //char buff[10];
-    //float d[9]; //tablica wartosci przed filtracja
-    //double D[9]; //tablica wartosci po filtracji
-    //float kf_update;
+
+
     imu.init();
     //imu.readData(d);
     //imu.filterData(d,D); 
     kf.reset();    
     pc.baud(115200);
 
-    triger1.attach(&task1, 0.0025);
-    triger2.attach(&task2, 0.05);
+    //triger1.attach(&task1, 0.0025);
+    //triger2.attach(&task2, 0.05);
+
+    
+    kalman_init(&pitch_data);
+    kalman_init(&roll_data);
+    
+    
+    
+    while (true) {   
+    
+    imu.readData(d);
+    r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
+    katx = acos(d[4]/r)*180/M_PI-90;
+    katy = acos(d[3]/r)*180/M_PI-90;
+    
     
-    while (true) {        
+    //Filtr Kalmana wlasny
+    // Perform filtering
+    kalman_innovate(&pitch_data, katx, d[0]*180/M_PI);
+    kalman_innovate(&roll_data, -katy, d[1]*180/M_PI);
+    // The angle is stored in state 1
+    pitchk2 = pitch_data.x1;
+    rollk2 = roll_data.x1;
+    
+    
+    //Filtr Buterwortha
+    //imu.filterData(d, D);
+    //sprintf(buff, "%f\n%f\r", d[3], D[3]);
+    
+    //Filtr Kalmana
+    kf_update = kf.update(d[1], -katy);
+    //Wyslanie wartosci w protekole szeregowym do Labview (zmienic liczbe danych w labView jest sprintf wysyla wiecej wartosci).
+    sprintf(buff, "%f,%f,%f\n\r", -katy, rollk2, kf_update);
+
+    pc.printf(buff);
+    wait(0.0035);
+         
         //pc.printf("%f\n%f\n%f\r", d[3], kf_update, kf_estimated_data);
-        wait(1.0f);
+        //wait(1.0f);
         // Komentarz wprowadzam w jezyku angielskim, poniewaz program jest dostepny publicznie:::
         // Hex char 0xD = \r is the termination char in the LabVIEW VISA Configure Serial Port vi.
         // It points to the end of string and separates single vector of acquired data. 
@@ -70,3 +117,5 @@
         //myled = !myled; // toggle a led
     }
 }
+
+