File content as of revision 3:795998b31c32:

/**
|**********************************************************************;
* 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>
#include <Thread.h>
#include "Liste.h"

using namespace std;

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_old = 0;
    counter=0;
    
    //Timer und Thread starten
    timer.start();
    periodic_task=true;
    thread.start(callback(this, &CalculateData::run));
    thread.set_priority(osPriorityHigh);
}

//Timer und Thread beenden
void CalculateData::disable(){
    periodic_task=false;
    timer.stop();
    thread.join();
}





//Periodischer Task
void CalculateData::run() {
    while(periodic_task){
        
        counter +=1;
        
        //Sensoren und Timer auslesen
        accelerometer->get_x_axes(buf_acc);
        acc_gyro->get_g_axes(buf_gyro);
        t[counter-1]=(double)timer.read_ms()/1000.0f;
        
        for(int i=0;i<3;i++){
            array_acc[i][counter-1]=buf_acc[i];
            array_gyro[i][counter-1]=buf_gyro[i];
        }

        Thread::wait(10);
    }
}



void CalculateData::getValue(Liste *list){
    
    //calculate actual Values
    for(int i=0;i<counter;i++){
        filterAcc(array_acc, i, acc);
        filterGyro(array_gyro, i, gyro);
        integrate(acc, acc_old, speed, t[i], t_old);
        integrate(gyro, gyro_old, angle, t[i], t_old);
        filterSpeed(speed, speed);
        integrate(speed, speed_old, pos, t[i], t_old);
        
        for (int j=0; j<3;j++){
            acc_old[j] = acc[j];
            gyro_old[j] = gyro[j];
            speed_old[j] = speed[j];
        }
        t_old=t[i];
    }
    counter = 0;
    
    //save Data in List
    list->accX= acc[0];
    list->accY= acc[1];
    list->accZ= acc[2];
    list->speedX= speed[0];
    list->speedY= speed[1];
    list->speedZ= speed[2];
    list->posX= pos[0];
    list->posY= pos[1];
    list->posZ= pos[2];
    list->gyroX= gyro[0];
    list->gyroY= gyro[1];
    list->gyroZ= gyro[2];
    list->angleX= angle[0];
    list->angleY= angle[1];
    list->angleZ= angle[2];
    list->time= t_old;
}









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




//FIR-Filter (Accelerometer)
void CalculateData::filterAcc(int array[3][30],int index, double *target){
    
    double sum_array[] = {0, 0, 0};
    
    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][index];
        sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)acc_filter[j][FILTERSIZE-1]);
        target[j] = sum_array[j];
    }        

}

//FIR-Filter (Gyro)
void CalculateData::filterGyro(int array[3][30],int index, double *target){
    double sum_array[] = {0, 0, 0};
    
    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][index];
        sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)gyro_filter[j][FILTERSIZE-1]);
        target[j] = sum_array[j];
    }
}

//FIR-Filter (Speed)
void CalculateData::filterSpeed(double *array, double *target){
    double sum_array[] = {0, 0, 0};
    
    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]);
        target[j] = sum_array[j];
    }
}



/*
//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];
    }
}
*/