Quadrocopter / Mbed 2 deprecated 2014_12_12_quadro_01

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of 2014_12_12_quadro_01 by Szewc Michał

main.cpp@10:2794020bfc0c, 2014-12-12 (annotated)

Committer:
Michu90
Date:
Fri Dec 12 10:15:10 2014 +0000
Revision:
10:2794020bfc0c
Parent:
9:3694ca4b48a7
Child:
11:15181dfe535e
a

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"
Michu90 10:2794020bfc0c 5 #include "kalman.h"
Michu90 10:2794020bfc0c 6
Michu90 10:2794020bfc0c 7 #define M_PI 3.141592
Michu90 10:2794020bfc0c 8 #define M_PI2 1.570796
Michu90 10:2794020bfc0c 9 #define dt 0.004
Michu90 10:2794020bfc0c 10
Michu90 10:2794020bfc0c 11 //kalman2
Michu90 10:2794020bfc0c 12 // Structs for containing filter data
Michu90 10:2794020bfc0c 13 kalman_data pitch_data;
Michu90 10:2794020bfc0c 14 kalman_data roll_data;
emilmont 0:f59179afee57 15
Pawel_13 8:7a22b8294c5d 16 //DigitalOut myled(LED_RED);
Kojto 4:d6816f97e349 17 Serial pc(USBTX, USBRX);
emilmont 0:f59179afee57 18
Pawel_13 8:7a22b8294c5d 19 IMU imu(PTE25,PTE24);
Pawel_13 8:7a22b8294c5d 20
Pawel_13 8:7a22b8294c5d 21 KalmanFilter kf;
Pawel_13 8:7a22b8294c5d 22
Michu90 10:2794020bfc0c 23 //Ticker triger1; //przerwanie filtracji
Michu90 10:2794020bfc0c 24 //Ticker triger2; //przerwanie wysyłania danych
Pawel_13 8:7a22b8294c5d 25
Pawel_13 8:7a22b8294c5d 26 float d[9];
Pawel_13 8:7a22b8294c5d 27 double D[9];
Pawel_13 8:7a22b8294c5d 28 float kf_update;
Michu90 10:2794020bfc0c 29 char buff[60];
Michu90 10:2794020bfc0c 30 float r,katx,katy;
Michu90 10:2794020bfc0c 31 float pitchk2, rollk2;
Pawel_13 8:7a22b8294c5d 32
Michu90 10:2794020bfc0c 33 /*
Pawel_13 8:7a22b8294c5d 34 void task1()
Pawel_13 8:7a22b8294c5d 35 {
Pawel_13 8:7a22b8294c5d 36 imu.readData(d);
Michu90 10:2794020bfc0c 37 r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
Michu90 10:2794020bfc0c 38 katx = acos(d[4]/r)*180/M_PI-90;
Pawel_13 8:7a22b8294c5d 39 //Filtr Buterwortha
Pawel_13 8:7a22b8294c5d 40 imu.filterData(d, D);
Pawel_13 8:7a22b8294c5d 41 //sprintf(buff, "%f\n%f\r", d[3], D[3]);
Pawel_13 8:7a22b8294c5d 42
Pawel_13 8:7a22b8294c5d 43 //Filtr Kalmana
Pawel_13 8:7a22b8294c5d 44 kf_update = kf.update(d[2], d[5]);
Pawel_13 9:3694ca4b48a7 45 //Wyslanie wartosci w protekole szeregowym do Labview (zmienic liczbe danych w labView jest sprintf wysyla wiecej wartosci).
Michu90 10:2794020bfc0c 46 sprintf(buff, "%f,%f,%f,%f\n\r", katx ,d[5], D[5], (float) kf_update);
Pawel_13 8:7a22b8294c5d 47 // sprintf(buff, "%f\n%f\n%f\r", d[6], d[7], d[8]);
Pawel_13 8:7a22b8294c5d 48 }
Pawel_13 8:7a22b8294c5d 49
Pawel_13 8:7a22b8294c5d 50 void task2()
Pawel_13 8:7a22b8294c5d 51 {
Pawel_13 8:7a22b8294c5d 52 pc.printf(buff);
Michu90 10:2794020bfc0c 53 }*/
Pawel_13 8:7a22b8294c5d 54
Pawel_13 8:7a22b8294c5d 55
sam_grove 7:986d5298b118 56 int main()
sam_grove 7:986d5298b118 57 {
Michu90 10:2794020bfc0c 58
Michu90 10:2794020bfc0c 59
Pawel_13 8:7a22b8294c5d 60 imu.init();
Pawel_13 8:7a22b8294c5d 61 //imu.readData(d);
Pawel_13 8:7a22b8294c5d 62 //imu.filterData(d,D);
Pawel_13 8:7a22b8294c5d 63 kf.reset();
Pawel_13 8:7a22b8294c5d 64 pc.baud(115200);
sam_grove 7:986d5298b118 65
Michu90 10:2794020bfc0c 66 //triger1.attach(&task1, 0.0025);
Michu90 10:2794020bfc0c 67 //triger2.attach(&task2, 0.05);
Michu90 10:2794020bfc0c 68
Michu90 10:2794020bfc0c 69
Michu90 10:2794020bfc0c 70 kalman_init(&pitch_data);
Michu90 10:2794020bfc0c 71 kalman_init(&roll_data);
Michu90 10:2794020bfc0c 72
Michu90 10:2794020bfc0c 73
Michu90 10:2794020bfc0c 74
Michu90 10:2794020bfc0c 75 while (true) {
Michu90 10:2794020bfc0c 76
Michu90 10:2794020bfc0c 77 imu.readData(d);
Michu90 10:2794020bfc0c 78 r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
Michu90 10:2794020bfc0c 79 katx = acos(d[4]/r)*180/M_PI-90;
Michu90 10:2794020bfc0c 80 katy = acos(d[3]/r)*180/M_PI-90;
Michu90 10:2794020bfc0c 81
Pawel_13 8:7a22b8294c5d 82
Michu90 10:2794020bfc0c 83 //Filtr Kalmana wlasny
Michu90 10:2794020bfc0c 84 // Perform filtering
Michu90 10:2794020bfc0c 85 kalman_innovate(&pitch_data, katx, d[0]*180/M_PI);
Michu90 10:2794020bfc0c 86 kalman_innovate(&roll_data, -katy, d[1]*180/M_PI);
Michu90 10:2794020bfc0c 87 // The angle is stored in state 1
Michu90 10:2794020bfc0c 88 pitchk2 = pitch_data.x1;
Michu90 10:2794020bfc0c 89 rollk2 = roll_data.x1;
Michu90 10:2794020bfc0c 90
Michu90 10:2794020bfc0c 91
Michu90 10:2794020bfc0c 92 //Filtr Buterwortha
Michu90 10:2794020bfc0c 93 //imu.filterData(d, D);
Michu90 10:2794020bfc0c 94 //sprintf(buff, "%f\n%f\r", d[3], D[3]);
Michu90 10:2794020bfc0c 95
Michu90 10:2794020bfc0c 96 //Filtr Kalmana
Michu90 10:2794020bfc0c 97 kf_update = kf.update(d[1], -katy);
Michu90 10:2794020bfc0c 98 //Wyslanie wartosci w protekole szeregowym do Labview (zmienic liczbe danych w labView jest sprintf wysyla wiecej wartosci).
Michu90 10:2794020bfc0c 99 sprintf(buff, "%f,%f,%f\n\r", -katy, rollk2, kf_update);
Michu90 10:2794020bfc0c 100
Michu90 10:2794020bfc0c 101 pc.printf(buff);
Michu90 10:2794020bfc0c 102 wait(0.0035);
Michu90 10:2794020bfc0c 103
Pawel_13 8:7a22b8294c5d 104 //pc.printf("%f\n%f\n%f\r", d[3], kf_update, kf_estimated_data);
Michu90 10:2794020bfc0c 105 //wait(1.0f);
Pawel_13 9:3694ca4b48a7 106 // Komentarz wprowadzam w jezyku angielskim, poniewaz program jest dostepny publicznie:::
Pawel_13 8:7a22b8294c5d 107 // Hex char 0xD = \r is the termination char in the LabVIEW VISA Configure Serial Port vi.
Pawel_13 8:7a22b8294c5d 108 // It points to the end of string and separates single vector of acquired data.
Pawel_13 8:7a22b8294c5d 109 // 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 110 // Then the float is encoded from the string and put into some array until all lines are processed.
Pawel_13 8:7a22b8294c5d 111 // The array elemnts are sent to the Waveform Chart to plot the data.
Pawel_13 8:7a22b8294c5d 112 // Using that syntax below, three series will be ploted.
Pawel_13 8:7a22b8294c5d 113 // pc.printf("%f\n%f\n%f\r", D[3],D[4],D[5]); //accelerations [m/s^2] from accel
Pawel_13 8:7a22b8294c5d 114 //pc.printf("%f\n%f\n%f\r", D[0],D[1],D[2]); //angular velocitites [rad/s] from gyro
Pawel_13 8:7a22b8294c5d 115 //pc.printf("%f\n%f\n%f\r", D[6],D[7],D[8]); //angle [rad] from magneto
Pawel_13 8:7a22b8294c5d 116 //
Pawel_13 8:7a22b8294c5d 117 //myled = !myled; // toggle a led
emilmont 0:f59179afee57 118 }
emilmont 0:f59179afee57 119 }
Michu90 10:2794020bfc0c 120
Michu90 10:2794020bfc0c 121