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@11:15181dfe535e, 2014-12-17 (annotated)

Committer:
Michu90
Date:
Wed Dec 17 08:03:00 2014 +0000
Revision:
11:15181dfe535e
Parent:
10:2794020bfc0c 
yea

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