A Team / Mbed 2 deprecated Polulu_IMU_Kalman

IMU Kalman Filter

Dependencies:   mbed

Fork of frdm_serial_K64F by Mechatronics

main.cpp@8:7a22b8294c5d, 2014-12-08 (annotated)

Committer:
Pawel_13
Date:
Mon Dec 08 13:21:33 2014 +0000
Revision:
8:7a22b8294c5d
Parent:
7:986d5298b118
Child:
9:3694ca4b48a7
Quadrocopter - Freescale k64F IMU and serial to NI LabView

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 8:7a22b8294c5d 30 sprintf(buff, "%f\n%f\n%f\r", d[5], D[5], (float) kf_update);
Pawel_13 8:7a22b8294c5d 31 // sprintf(buff, "%f\n%f\n%f\r", d[6], d[7], d[8]);
Pawel_13 8:7a22b8294c5d 32 }
Pawel_13 8:7a22b8294c5d 33
Pawel_13 8:7a22b8294c5d 34 void task2()
Pawel_13 8:7a22b8294c5d 35 {
Pawel_13 8:7a22b8294c5d 36 pc.printf(buff);
Pawel_13 8:7a22b8294c5d 37 }
Pawel_13 8:7a22b8294c5d 38
Pawel_13 8:7a22b8294c5d 39
sam_grove 7:986d5298b118 40 int main()
sam_grove 7:986d5298b118 41 {
Pawel_13 8:7a22b8294c5d 42 //char buff[10];
Pawel_13 8:7a22b8294c5d 43 //float d[9]; //tablica wartosci przed filtracja
Pawel_13 8:7a22b8294c5d 44 //double D[9]; //tablica wartosci po filtracji
Pawel_13 8:7a22b8294c5d 45 //float kf_update;
Pawel_13 8:7a22b8294c5d 46 imu.init();
Pawel_13 8:7a22b8294c5d 47 //imu.readData(d);
Pawel_13 8:7a22b8294c5d 48 //imu.filterData(d,D);
Pawel_13 8:7a22b8294c5d 49 kf.reset();
Pawel_13 8:7a22b8294c5d 50 pc.baud(115200);
sam_grove 7:986d5298b118 51
Pawel_13 8:7a22b8294c5d 52 triger1.attach(&task1, 0.0025);
Pawel_13 8:7a22b8294c5d 53 triger2.attach(&task2, 0.05);
Pawel_13 8:7a22b8294c5d 54
Pawel_13 8:7a22b8294c5d 55 while (true) {
Pawel_13 8:7a22b8294c5d 56 //pc.printf("%f\n%f\n%f\r", d[3], kf_update, kf_estimated_data);
Pawel_13 8:7a22b8294c5d 57 wait(1.0f);
Pawel_13 8:7a22b8294c5d 58 // Hex char 0xD = \r is the termination char in the LabVIEW VISA Configure Serial Port vi.
Pawel_13 8:7a22b8294c5d 59 // It points to the end of string and separates single vector of acquired data.
Pawel_13 8:7a22b8294c5d 60 // 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 61 // Then the float is encoded from the string and put into some array until all lines are processed.
Pawel_13 8:7a22b8294c5d 62 // The array elemnts are sent to the Waveform Chart to plot the data.
Pawel_13 8:7a22b8294c5d 63 // Using that syntax below, three series will be ploted.
Pawel_13 8:7a22b8294c5d 64 // pc.printf("%f\n%f\n%f\r", D[3],D[4],D[5]); //accelerations [m/s^2] from accel
Pawel_13 8:7a22b8294c5d 65 //pc.printf("%f\n%f\n%f\r", D[0],D[1],D[2]); //angular velocitites [rad/s] from gyro
Pawel_13 8:7a22b8294c5d 66 //pc.printf("%f\n%f\n%f\r", D[6],D[7],D[8]); //angle [rad] from magneto
Pawel_13 8:7a22b8294c5d 67 //
Pawel_13 8:7a22b8294c5d 68 //myled = !myled; // toggle a led
emilmont 0:f59179afee57 69 }
emilmont 0:f59179afee57 70 }