File content as of revision 10:2794020bfc0c:

#include "stdio.h"
#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);

IMU imu(PTE25,PTE24);

KalmanFilter kf;

//Ticker triger1; //przerwanie filtracji
//Ticker triger2; //przerwanie wysyłania danych

float d[9];
double D[9];
float kf_update;
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]);
    
    //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,%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()
{


    imu.init();
    //imu.readData(d);
    //imu.filterData(d,D); 
    kf.reset();    
    pc.baud(115200);

    //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;
    
    
    //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);
        // 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. 
        // Hex char 0xA = \n points to the new line. LabVIEW Pick Line vi selects the line with a float represented by string.
        // Then the float is encoded from the string and put into some array until all lines are processed.
        // The array elemnts are sent to the Waveform Chart to plot the data.
        // Using that syntax below, three series will be ploted.
        // pc.printf("%f\n%f\n%f\r", D[3],D[4],D[5]); //accelerations [m/s^2] from accel
        //pc.printf("%f\n%f\n%f\r", D[0],D[1],D[2]); //angular velocitites [rad/s] from gyro
        //pc.printf("%f\n%f\n%f\r", D[6],D[7],D[8]); //angle [rad] from magneto
        //
        //myled = !myled; // toggle a led
    }
}