File content as of revision 8:7a22b8294c5d:

#include "stdio.h"
#include "mbed.h"
#include "IMU.h"
#include "KalmanFilter.h"

//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[40];

void task1()
{
    imu.readData(d);
    //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]);
    sprintf(buff, "%f\n%f\n%f\r", 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()
{
    //char buff[10];
    //float d[9]; //tablica wartosci przed filtracja
    //double D[9]; //tablica wartosci po filtracji
    //float kf_update;
    imu.init();
    //imu.readData(d);
    //imu.filterData(d,D); 
    kf.reset();    
    pc.baud(115200);

    triger1.attach(&task1, 0.0025);
    triger2.attach(&task2, 0.05);
    
    while (true) {        
        //pc.printf("%f\n%f\n%f\r", d[3], kf_update, kf_estimated_data);
        wait(1.0f);
        // 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
    }
}