File content as of revision 11:15181dfe535e:

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

#define M_PI 3.141592
#define M_PI2 1.570796
#define dt 0.004

//kalman
// 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);
Offsets off;

//KalmanFilter kf;

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

float d[9];
double D[9];
float o[3];
float O[3];
float kf_update;
char buff[120];
float r,katx,katy;
float pitch, roll;
float pitch2, roll2;
char odczyt[20];
char znak;
int i;
float offsetGyro[3];


void task1()
{
    imu.readData(d);
    imu.filterData(d, D);
    off.offsetData(d,offsetGyro,o);
    off.offsetData2(D,offsetGyro,O);
    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 Buterwortha
    //sprintf(buff, "%f\n%f\r", d[3], D[3]);
    
    //Filtr Kalmana
    kalman_innovate(&pitch_data, katx, o[0]*180/M_PI);
    kalman_innovate(&roll_data, -katy, o[1]*180/M_PI);
    pitch = pitch_data.x1;
    roll = roll_data.x1;
    
    //Filtr Kalmana butterworth 2nd
    kalman_innovate(&pitch_data, katx, O[0]*180/M_PI);
    kalman_innovate(&roll_data, -katy, O[1]*180/M_PI);
    pitch2 = pitch_data.x1;
    roll2 = roll_data.x1;
}


void task2()
{
    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);
    pc.printf(buff);
}

        
int main()
{


    imu.init();
    pc.baud(115200);
    off.setOffsets(offsetGyro, pc, imu);

    kalman_init(&pitch_data);
    kalman_init(&roll_data);
    
    /*    
    for (i=0;i<1000;i++){
        imu.readData(d);
        off.offsetData(d,offsetGyro,o);
        sprintf(buff, "%f,%f,%f\n\r", (o[0]*180/M_PI), (o[1]*180/M_PI), (o[2]*180/M_PI) );
        pc.printf(buff);
        }*/
    
    triger1.attach(&task1, 0.005);
    triger2.attach(&task2, 0.02);
    
    
    while (true) {   
   
    wait(1.0f);
         
        //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
    }
}