A Team / Mbed 2 deprecated Polulu_IMU_Kalman

IMU Kalman Filter

Dependencies:   mbed

Fork of frdm_serial_K64F by Mechatronics

main.cpp@9:3694ca4b48a7, 2014-12-08 (annotated)

Committer:
Pawel_13
Date:
Mon Dec 08 13:28:17 2014 +0000
Revision:
9:3694ca4b48a7
Parent:
8:7a22b8294c5d
Child:
10:444b09c1798d
Some comments were added.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Pawel_13 8:7a22b8294c5d 1 #include "stdio.h"
emilmont 0:f59179afee57 2 #include "mbed.h"
Pawel_13 8:7a22b8294c5d 3 #include "IMU.h"
Pawel_13 8:7a22b8294c5d 4 #include "KalmanFilter.h"
emilmont 0:f59179afee57 5
Pawel_13 8:7a22b8294c5d 6 //DigitalOut myled(LED_RED);
Kojto 4:d6816f97e349 7 Serial pc(USBTX, USBRX);
emilmont 0:f59179afee57 8
Pawel_13 8:7a22b8294c5d 9 IMU imu(PTE25,PTE24);
Pawel_13 8:7a22b8294c5d 10
Pawel_13 8:7a22b8294c5d 11 KalmanFilter kf;
Pawel_13 8:7a22b8294c5d 12
Pawel_13 8:7a22b8294c5d 13 Ticker triger1; //przerwanie filtracji
Pawel_13 8:7a22b8294c5d 14 Ticker triger2; //przerwanie wysyłania danych
Pawel_13 8:7a22b8294c5d 15
Pawel_13 8:7a22b8294c5d 16 float d[9];
Pawel_13 8:7a22b8294c5d 17 double D[9];
Pawel_13 8:7a22b8294c5d 18 float kf_update;
Pawel_13 8:7a22b8294c5d 19 char buff[40];
Pawel_13 8:7a22b8294c5d 20
Pawel_13 8:7a22b8294c5d 21 void task1()
Pawel_13 8:7a22b8294c5d 22 {
Pawel_13 8:7a22b8294c5d 23 imu.readData(d);
Pawel_13 8:7a22b8294c5d 24 //Filtr Buterwortha
Pawel_13 8:7a22b8294c5d 25 imu.filterData(d, D);
Pawel_13 8:7a22b8294c5d 26 //sprintf(buff, "%f\n%f\r", d[3], D[3]);
Pawel_13 8:7a22b8294c5d 27
Pawel_13 8:7a22b8294c5d 28 //Filtr Kalmana
Pawel_13 8:7a22b8294c5d 29 kf_update = kf.update(d[2], d[5]);
Pawel_13 9:3694ca4b48a7 30 //Wyslanie wartosci w protekole szeregowym do Labview (zmienic liczbe danych w labView jest sprintf wysyla wiecej wartosci).
Pawel_13 8:7a22b8294c5d 31 sprintf(buff, "%f\n%f\n%f\r", d[5], D[5], (float) kf_update);
Pawel_13 8:7a22b8294c5d 32 // sprintf(buff, "%f\n%f\n%f\r", d[6], d[7], d[8]);
Pawel_13 8:7a22b8294c5d 33 }
Pawel_13 8:7a22b8294c5d 34
Pawel_13 8:7a22b8294c5d 35 void task2()
Pawel_13 8:7a22b8294c5d 36 {
Pawel_13 8:7a22b8294c5d 37 pc.printf(buff);
Pawel_13 8:7a22b8294c5d 38 }
Pawel_13 8:7a22b8294c5d 39
Pawel_13 8:7a22b8294c5d 40
sam_grove 7:986d5298b118 41 int main()
sam_grove 7:986d5298b118 42 {
Pawel_13 8:7a22b8294c5d 43 //char buff[10];
Pawel_13 8:7a22b8294c5d 44 //float d[9]; //tablica wartosci przed filtracja
Pawel_13 8:7a22b8294c5d 45 //double D[9]; //tablica wartosci po filtracji
Pawel_13 8:7a22b8294c5d 46 //float kf_update;
Pawel_13 8:7a22b8294c5d 47 imu.init();
Pawel_13 8:7a22b8294c5d 48 //imu.readData(d);
Pawel_13 8:7a22b8294c5d 49 //imu.filterData(d,D);
Pawel_13 8:7a22b8294c5d 50 kf.reset();
Pawel_13 8:7a22b8294c5d 51 pc.baud(115200);
sam_grove 7:986d5298b118 52
Pawel_13 8:7a22b8294c5d 53 triger1.attach(&task1, 0.0025);
Pawel_13 8:7a22b8294c5d 54 triger2.attach(&task2, 0.05);
Pawel_13 8:7a22b8294c5d 55
Pawel_13 8:7a22b8294c5d 56 while (true) {
Pawel_13 8:7a22b8294c5d 57 //pc.printf("%f\n%f\n%f\r", d[3], kf_update, kf_estimated_data);
Pawel_13 8:7a22b8294c5d 58 wait(1.0f);
Pawel_13 9:3694ca4b48a7 59 // Komentarz wprowadzam w jezyku angielskim, poniewaz program jest dostepny publicznie:::
Pawel_13 8:7a22b8294c5d 60 // Hex char 0xD = \r is the termination char in the LabVIEW VISA Configure Serial Port vi.
Pawel_13 8:7a22b8294c5d 61 // It points to the end of string and separates single vector of acquired data.
Pawel_13 8:7a22b8294c5d 62 // Hex char 0xA = \n points to the new line. LabVIEW Pick Line vi selects the line with a float represented by string.
Pawel_13 8:7a22b8294c5d 63 // Then the float is encoded from the string and put into some array until all lines are processed.
Pawel_13 8:7a22b8294c5d 64 // The array elemnts are sent to the Waveform Chart to plot the data.
Pawel_13 8:7a22b8294c5d 65 // Using that syntax below, three series will be ploted.
Pawel_13 8:7a22b8294c5d 66 // pc.printf("%f\n%f\n%f\r", D[3],D[4],D[5]); //accelerations [m/s^2] from accel
Pawel_13 8:7a22b8294c5d 67 //pc.printf("%f\n%f\n%f\r", D[0],D[1],D[2]); //angular velocitites [rad/s] from gyro
Pawel_13 8:7a22b8294c5d 68 //pc.printf("%f\n%f\n%f\r", D[6],D[7],D[8]); //angle [rad] from magneto
Pawel_13 8:7a22b8294c5d 69 //
Pawel_13 8:7a22b8294c5d 70 //myled = !myled; // toggle a led
emilmont 0:f59179afee57 71 }
emilmont 0:f59179afee57 72 }