File content as of revision 6:4641f2b2684b:

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

using namespace std;

static XNucleoIKS01A3       *mems_expansion_board;              //Sensor-Board
static LSM6DSOSensor        *acc_gyro;                          //Gyro-Sensor
static LIS2DW12Sensor       *accelerometer;                     //Acceleroation-Sensor
static LIS2MDLSensor        *magnetometer;                      //Magnetometer
const float                 accOffset[3] = {-0.007734011220907f, -0.007085474965694f, -9.910594323583762f};
const int                   FILTERSIZE = 28;
static float                acc_filter[3][FILTERSIZE];
static float                gyro_filter[3][FILTERSIZE];
static float                speed_filter[3][FILTERSIZE];
float                       magtransformationmatrix[3][3]={{1.624f, 0.049f, 0.011f}, {-0.004f, 1.668f, 0.001f}, {0.074f, 0.001f, 1.635f}};
float                       bias[3]={-466.649f, -273.636f, 58.044f};
const float                 filter_koef[FILTERSIZE] = {1.54459639027449f, -5.19311640449656f, -2.26968649197199f, -1.43490906020467f,
                                                        -0.996336387257677f, -0.507518334242666f, 0.129727243968288f, 0.905406882193463f,
                                                        1.76820384311742f, 2.66200095951371f, 3.49445629978250f, 4.20214691216170f,
                                                        4.71327076441993f, 4.98175738274206f, 4.98175738274206f, 4.71327076441993f,
                                                        4.20214691216170f, 3.49445629978250f, 2.66200095951371f, 1.76820384311742f,
                                                        0.905406882193463f, 0.129727243968288f, -0.507518334242666f, -0.996336387257677f,
                                                        -1.43490906020467f, -2.26968649197199f, -5.19311640449656f, 1.54459639027449f};



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;
    magnetometer = mems_expansion_board->magnetometer;
    
    /* Enable all sensors */
    accelerometer->enable_x();
    acc_gyro->enable_x();
    acc_gyro->enable_g();
    magnetometer->enable();
    
    //initialisation Mahony
    mahony = new MahonyAHRS(200.0);
}


CalculateData::~CalculateData() {

}




void CalculateData::enable(){
    //Anfangsbedingungen auf NULL
    for (int i = 0; i<3; i++){
        acc[i]=0;
        acc_old[i]=0;
        speed[i]=0;
        speed_old[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() {
    
    int buf_acc[3];
    int buf_gyro[3];
    int buf_mag[3];
    
    while(periodic_task){
        
        counter +=1;
        
        //Sensoren und Timer auslesen
        //accelerometer->get_x_axes(buf_acc);
        acc_gyro->get_x_axes(buf_acc);
        acc_gyro->get_g_axes(buf_gyro);
        magnetometer->get_m_axes(buf_mag);
        t[counter-1]=(float)timer.read_ms();
        
        for(int i=0;i<3;i++){
            array_acc[i][counter-1]=buf_acc[i];
            array_gyro[i][counter-1]=buf_gyro[i];
            array_mag[i][counter-1]=buf_mag[i];
        }

        Thread::wait(2);
    }
}



void CalculateData::getValue(Liste *list){
    //udates the Values
    update();
    
    //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->time= t_old;
}


//calculate actual Values
void CalculateData::update(){
    
    for(int i=0;i<counter;i++){
        
        float buff[3];
        
        calibrationMag(array_mag[0][i],array_mag[1][i],array_mag[2][i], buff);
                
        mahony->update(-array_gyro[2][i]*PI/180000.0f, array_gyro[0][i]*PI/180000.0f, array_gyro[1][i]*PI/180000.0f,
        -array_acc[2][i]/1000.0f, array_acc[0][i]/1000.0f, array_acc[1][i]/1000.0f,
        buff[2], buff[0], -buff[1]);
        
        mahony->getEuler();
        transform(array_acc, i, acc, mahony->getRoll(), mahony->getPitch(), mahony->getYaw());
        
        filterAcc(acc);
        
        integrate(acc, acc_old, speed, t[i], t_old); //in mm/s
        
        //filterSpeed(speed);
        
        integrate(speed, speed_old, pos, t[i], t_old); //in um
        
        for (int j=0; j<3;j++){
            acc_old[j] = acc[j];
            speed_old[j] = speed[j];
        }
        t_old=t[i];
    }
    counter = 0;
    printf("Roll:\t%i\tPitch:\t%i\tYaw:\t%i\r\n",mahony->getRoll(),mahony->getPitch(),mahony->getYaw());
    //printf("%f, %f, %f, %f, %f, %f, %f\r\n",acc[0],acc[1],acc[2], t_old/1000.0f, speed[0]/1000.0f,speed[1]/1000.0f,speed[2]/1000.0f);
    //printf("%f, %f, %f\r\n",acc[0],acc[1],acc[2]);
}






//Daten Integrieren nach expl. Euler
void CalculateData::integrate(float *x, float *x_old, float *y, float t, float t_old){
    float 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(float *array){
    
    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] * (float)acc_filter[j][i]);
        }
        acc_filter[j][FILTERSIZE-1] = array[j];
        sum_array[j] += (filter_koef[FILTERSIZE-1] * (float)acc_filter[j][FILTERSIZE-1]);
        array[j] = sum_array[j]/FILTERSIZE;
    }        

}


//FIR-Filter (Speed)
void CalculateData::filterSpeed(float *array){
    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] * (float)speed_filter[j][i]);
        }
        speed_filter[j][FILTERSIZE-1] = array[j];
        sum_array[j] += (filter_koef[FILTERSIZE-1] * (float)speed_filter[j][FILTERSIZE-1]);
        array[j] = sum_array[j]/FILTERSIZE;
    }
}


void CalculateData::calibrationMag(int x,int y,int z, float *buff){
    buff[0] = magtransformationmatrix[0][0]*((float)x-bias[0])+
    magtransformationmatrix[0][1]*((float)y-bias[1])+
    magtransformationmatrix[0][2]*((float)z-bias[2]);
    
    buff[1] = magtransformationmatrix[1][0]*((float)x-bias[0])+
    magtransformationmatrix[1][1]*((float)y-bias[1])+
    magtransformationmatrix[1][2]*((float)z-bias[2]);
    
    buff[2] = magtransformationmatrix[2][0]*((float)x-bias[0])+
    magtransformationmatrix[2][1]*((float)y-bias[1])+
    magtransformationmatrix[2][2]*((float)z-bias[2]);
}


//Beschleunigung in globale Koordinaten Transformieren
void CalculateData::transform(int old_val[3][100], int index, float *new_val, int roll, int pitch, int yaw){
    
    //Koeffizienten zum umrechnen
    float sa = sin((float)yaw*PI/180.0f);    
    float ca = cos((float)yaw*PI/180.0f);
    float sb = sin((float)(-pitch)*PI/180.0f);
    float cb = cos((float)(-pitch)*PI/180.0f);
    float sc = sin((float)roll*PI/180.0f);
    float cc = cos((float)roll*PI/180.0f);
    
    //Transformation
    /*new_val[0] = -(float)old_val[2][index]*(ca*cb) - (float)old_val[0][index]*(ca*sb*sc-cc*sa) + (float)old_val[1][index]*(ca*cc*sb+sa*sc);
    new_val[1] = -(float)old_val[2][index]*(cb*sa) - (float)old_val[0][index]*(ca*cc+sa*sb*sc) + (float)old_val[1][index]*(cc*sa*sb-ca*sc);
    new_val[2] = -(float)old_val[2][index]*(-sb) - (float)old_val[0][index]*(cb*sc) + (float)old_val[1][index]*(cb*cc);
    */
    new_val[0] = (-(float)old_val[2][index]*(ca*cb) + (float)old_val[0][index]*(cb*sa) + (float)old_val[1][index]*(-sb))*0.0098f - accOffset[0];
    new_val[1] = (-(float)old_val[2][index]*(ca*sb*sc-cc*sa) + (float)old_val[0][index]*(ca*cc+sa*sb*sc) + (float)old_val[1][index]*(cb*sc))*0.0098f - accOffset[1];
    new_val[2] = (-(float)old_val[2][index]*(ca*cc*sb+sa*sc) + (float)old_val[0][index]*(cc*sa*sb-ca*sc) + (float)old_val[1][index]*(cb*cc))*0.0098f - accOffset[2];
}