File content as of revision 2:4cccdc792719:

/**
|**********************************************************************;
* Project           : Projektarbeit Systemtechnik PES4
*
* Program name      : Beispiel
*
* Author            : PES4 Team1
*
* Team              : **Team 1**
*                     Fabio Bernard
*                     Lukas Egli
*                     Matthias Ott
*                     Pascal Novacki
*                     Robin Wanner
*                     Vincent Vescoli
*                     Cyrill Zoller
*
* Date created      : 20.02.2020
*
* Purpose           : Beispiel
*
|**********************************************************************;
**/

#define  PI (3.14159265)
#include "CalculateData.h"
#include "mbed.h"
#include "XNucleoIKS01A3.h"
#include <math.h>

using namespace std;

const float                 CalculateData::PERIOD = 1.0f;       //Periode von 10 ms
static XNucleoIKS01A3       *mems_expansion_board;              //Sensor-Board
static LSM6DSOSensor        *acc_gyro;                          //Gyro-Sensor
static LIS2DW12Sensor       *accelerometer;                     //Acceleroation-Sensor
const int                   FILTERSIZE = 28;
static int                  acc_filter[3][FILTERSIZE];
static int                  gyro_filter[3][FILTERSIZE];
static int                  speed_filter[3][FILTERSIZE];
const double                filter_koef[FILTERSIZE] = {0.04002232346037,  -0.1345598020407, -0.05881026752833, -0.03718019471321,
                                                        -0.02581625686635, -0.01315040166028, 0.003361386671892,   0.0234601656009,
                                                        0.04581625763125,  0.06897560043795,   0.0905455051097,   0.1088826077838,
                                                        0.1221264326899,   0.1290832392388,   0.1290832392388,   0.1221264326899,
                                                        0.1088826077838,   0.0905455051097,  0.06897560043795,  0.04581625763125,
                                                        0.0234601656009, 0.003361386671892, -0.01315040166028, -0.02581625686635,
                                                        -0.03718019471321, -0.05881026752833,  -0.1345598020407,  0.04002232346037
                                                        };





CalculateData::CalculateData(PinName p0, PinName p1, PinName p2, PinName p3,
                            PinName p4, PinName p5, PinName p6){                           
    /* Instantiate the expansion board */
    mems_expansion_board = XNucleoIKS01A3::instance(p0, p1, p2, p3, p4, p5, p6);

    /* Retrieve the composing elements of the expansion board */
    acc_gyro = mems_expansion_board->acc_gyro;
    accelerometer = mems_expansion_board->accelerometer;
    
    /* Enable all sensors */
    accelerometer->enable_x();
    //acc_gyro->enable_x();
    acc_gyro->enable_g();
}


CalculateData::~CalculateData() {

}




void CalculateData::enable(){
    //Anfangsbedingungen auf NULL
    for (int i = 0; i<3; i++){
        acc[i]=0;
        acc_old[i]=0;
        gyro[i]=0;
        gyro_old[i]=0;
        speed[i]=0;
        speed_old[i]=0;
        angle[i]=0;
        pos[i]=0; 
    }
    
    t = 0;
    t_old = 0;
    
    //Timer und Ticker starten
    timer.start();
    ticker.attach(callback(this, &CalculateData::run), PERIOD);
}

//Timer und Ticker beenden
void CalculateData::disable(){
    //ticker.detach();
    timer.stop();    
}





//Periodischer Task
void CalculateData::run() {
    //Letzte Werte speichern
    for (int i=0; i<3;i++){
        acc_old[i] = acc[i];
        gyro_old[i] = gyro[i];
        speed_old[i] = speed[i];
    }
    t_old = t;
    
    //Sensoren und Timer auslesen
    accelerometer->get_x_axes(acc);
    acc_gyro->get_g_axes(gyro);
    t=timer.read_ms();

    filter(acc, 1);                                 //Daten Filtern
    filter(gyro, 2);                                //Daten Filtern
    integrate(gyro, gyro_old, angle, t, t_old);     //Integral -> Winkel
    //transform(acc, angle);                          //Beschleunigung in Globale Koordinaten transformieren
    integrate(acc, acc_old, speed, t, t_old);       //Integral -> Geschwindigkeit
    filter(speed, 3);                               //Daten Filtern
    integrate(speed, speed_old, pos, t, t_old);     //Integral -> Position
}


//Daten Integrieren nach expl. Euler
void CalculateData::integrate(int *x, int *x_old, int *y, int t, int t_old){
    int dt = t-t_old;
    
    for(int i = 0;i < 3;i++){
        y[i] = (uint32_t)((double)y[i] + ((double)(x[i] + (double)x_old[i])/2.0)*dt/1000.0);
    }
        
}




//Filtern der Daten mittels FIR-Filter
void CalculateData::filter(int *array, int type){
    
    double sum_array[] = {0, 0, 0};
    
    switch(type){
        
        case 1:
                for(int j=0;j<3;j++){
                    for(int i=0;i<FILTERSIZE-2;i++){
                        acc_filter[j][i] = acc_filter[j][i+1];
                        sum_array[j] += (filter_koef[i] * (double)acc_filter[j][i]);
                    }
                    acc_filter[j][FILTERSIZE-1] = array[j];
                    sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)acc_filter[j][FILTERSIZE-1]);
                    array[j] = (int)sum_array[j];
                }        
        break;
        
        case 2:
                for(int j=0;j<3;j++){
                    for(int i=0;i<FILTERSIZE-2;i++){
                        gyro_filter[j][i] = gyro_filter[j][i+1];
                        sum_array[j] += (filter_koef[i] * (double)gyro_filter[j][i]);
                    }
                    gyro_filter[j][FILTERSIZE-1] = array[j];
                    sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)gyro_filter[j][FILTERSIZE-1]);
                    array[j] = (int)sum_array[j];
                }        
        break;
        
        case 3:
                for(int j=0;j<3;j++){
                    for(int i=0;i<FILTERSIZE-2;i++){
                        speed_filter[j][i] = speed_filter[j][i+1];
                        sum_array[j] += (filter_koef[i] * (double)speed_filter[j][i]);
                    }
                    speed_filter[j][FILTERSIZE-1] = array[j];
                    sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)speed_filter[j][FILTERSIZE-1]);
                    array[j] = (int)sum_array[j];
                }
        default:
        break;  
    }
}




void CalculateData::getAccelerometer(int *array){
    for(int i=0;i<3;i++){
        array[i] = acc[i];
    }    
}


void CalculateData::getSpeed(int *array){
    for(int i=0;i<3;i++){
        array[i] = speed[i];
    } 
}


void CalculateData::getPosition(int *array){
    for(int i=0;i<3;i++){
        array[i] = pos[i];
    } 
}


void CalculateData::getGyro(int *array){
    for(int i=0;i<3;i++){
        array[i] = gyro[i];
    } 
}


void CalculateData::getAngle(int *array){
    for(int i=0;i<3;i++){
        array[i] = angle[i];
    } 
}


/*
//Beschleunigung in globale Koordinaten Transformieren
void CalculateData::transform(int *acc, int *angle){
    double acc_new[3];                      //Neue Werte
    double a = angle[0]*PI/(1000.0*180.0);  //Winkel alpha in rad
    double b = angle[1]*PI/(1000.0*180.0);  //Winkel beta in rad
    double c = angle[2]*PI/(1000.0*180.0);  //Winkel gamma in rad
    
    //Transformation
    acc_new[0] = (double)acc[0]*cos(a)*cos(c) - (double)acc[1]*cos(b)*sin(c) + (double)acc[2]*sin(b);
    acc_new[1] = (double)acc[0]*(cos(a)*sin(c)+sin(a)*sin(b)*cos(c)) + (double)acc[1]*(cos(a)*cos(c)-sin(a)*sin(b)*cos(c)) - (double)acc[2]*sin(a)*cos(b);
    acc_new[2] = (double)acc[0]*(sin(a)*sin(c)-cos(a)*sin(b)*cos(c)) - (double)acc[1]*(sin(a)*cos(c)+cos(a)*sin(b)*sin(c)) + (double)acc[2]*cos(a)*cos(b);
    
    for(int i=0;i<3;i++){
        acc[i] = (int)acc_new[i];
    }
}
*/