File content as of revision 1:48e219526d0f:

/**
|**********************************************************************;
* 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
const int                   FILTERSIZE = 10;
static XNucleoIKS01A3       *mems_expansion_board;              //Sensor-Board
static LSM6DSOSensor        *acc_gyro;                          //Gyro-Sensor
static LIS2DW12Sensor       *accelerometer;                     //Acceleroation-Sensor
static int                  acc_filter[3][FILTERSIZE];
static int                  gyro_filter[3][FILTERSIZE];
const double                filter_koef[] = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1};




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(){
    //Mittelwert der Fehler der Sensoren mit g berechnen
    acc_err = {0, 0, 0};
    gyro_err = {0, 0, 0};
    for(int i=0;i<10;i++){
        accelerometer->get_x_axes(acc);
        acc_gyro->get_g_axes(gyro);
        for(int k=0;k<3;k++){
            acc_err[i] += acc[i];
            gyro_err[i] += gyro[i];
        }
    }
    
    for(int i=0; i<3; i++){
        acc_err[i] /= 10;
        gyro_err[i] /= 10;
    }
    
    //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, gyro);                              //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
    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] = y[i] + ((x[i] + x_old[i])/2)*dt;
    }
        
}

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


//Filtern der Daten mittels FIR-Filter
void CalculateData::filter(int *acc, int *gyro){
    double sum_acc[] = {0, 0, 0};
    double sum_gyro[] = {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];
            gyro_filter[j][i] = gyro_filter[j][i+1];
            sum_acc[j] += (filter_koef[i] * (double)acc_filter[j][i]);
            sum_gyro[j] += (filter_koef[i] * (double)gyro_filter[j][i]); 
        }
        acc_filter[j][FILTERSIZE-1] = acc[j];
        gyro_filter[j][FILTERSIZE-1] = gyro[j];
        
        sum_acc[j] += (filter_koef[FILTERSIZE-1] * (double)acc_filter[j][FILTERSIZE-1]);
        sum_gyro[j] += (filter_koef[FILTERSIZE-1] * (double)gyro_filter[j][FILTERSIZE-1]);
        
        acc[j] = (int)sum_acc[j];
        gyro[j] = (int)sum_gyro[j];
    }        
}




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