CalculateData.cpp

00001 /**
00002 |**********************************************************************;
00003 * Project           : Projektarbeit Systemtechnik PES4
00004 *
00005 * Program name      : Beispiel
00006 *
00007 * Author            : PES4 Team1
00008 *
00009 * Team              : **Team 1**
00010 *                     Fabio Bernard
00011 *                     Lukas Egli
00012 *                     Matthias Ott
00013 *                     Pascal Novacki
00014 *                     Robin Wanner
00015 *                     Vincent Vescoli
00016 *                     Cyrill Zoller
00017 *
00018 * Date created      : 20.02.2020
00019 *
00020 * Purpose           : Beispiel
00021 *
00022 |**********************************************************************;
00023 **/
00024 
00025 #define  PI (3.14159265)
00026 #include "CalculateData.h"
00027 #include "mbed.h"
00028 #include "XNucleoIKS01A3.h"
00029 #include <math.h>
00030 #include <Thread.h>
00031 #include "Liste.h"
00032 #include "MahonyAHRS.h"
00033 
00034 using namespace std;
00035 
00036 static XNucleoIKS01A3       *mems_expansion_board;              //Sensor-Board
00037 static LSM6DSOSensor        *acc_gyro;                          //Gyro-Sensor
00038 static LIS2DW12Sensor       *accelerometer;                     //Acceleroation-Sensor
00039 static LIS2MDLSensor        *magnetometer;                      //Magnetometer
00040 const float                 accOffset[3] = {-0.007734011220907f, -0.007085474965694f, -9.910594323583762f};
00041 const int                   FILTERSIZE = 28;
00042 static float                acc_filter[3][FILTERSIZE];
00043 static float                gyro_filter[3][FILTERSIZE];
00044 static float                speed_filter[3][FILTERSIZE];
00045 float                       magtransformationmatrix[3][3]={{1.624f, 0.049f, 0.011f}, {-0.004f, 1.668f, 0.001f}, {0.074f, 0.001f, 1.635f}};
00046 float                       bias[3]={-466.649f, -273.636f, 58.044f};
00047 const float                 filter_koef[FILTERSIZE] = {1.54459639027449f, -5.19311640449656f, -2.26968649197199f, -1.43490906020467f,
00048                                                         -0.996336387257677f, -0.507518334242666f, 0.129727243968288f, 0.905406882193463f,
00049                                                         1.76820384311742f, 2.66200095951371f, 3.49445629978250f, 4.20214691216170f,
00050                                                         4.71327076441993f, 4.98175738274206f, 4.98175738274206f, 4.71327076441993f,
00051                                                         4.20214691216170f, 3.49445629978250f, 2.66200095951371f, 1.76820384311742f,
00052                                                         0.905406882193463f, 0.129727243968288f, -0.507518334242666f, -0.996336387257677f,
00053                                                         -1.43490906020467f, -2.26968649197199f, -5.19311640449656f, 1.54459639027449f};
00054 
00055 
00056 
00057 CalculateData::CalculateData(PinName p0, PinName p1, PinName p2, PinName p3,
00058                             PinName p4, PinName p5, PinName p6){                           
00059     /* Instantiate the expansion board */
00060     mems_expansion_board = XNucleoIKS01A3::instance(p0, p1, p2, p3, p4, p5, p6);
00061 
00062     /* Retrieve the composing elements of the expansion board */
00063     acc_gyro = mems_expansion_board->acc_gyro;
00064     accelerometer = mems_expansion_board->accelerometer;
00065     magnetometer = mems_expansion_board->magnetometer;
00066     
00067     /* Enable all sensors */
00068     accelerometer->enable_x();
00069     acc_gyro->enable_x();
00070     acc_gyro->enable_g();
00071     magnetometer->enable();
00072     
00073     //initialisation Mahony
00074     mahony = new MahonyAHRS(200.0);
00075 }
00076 
00077 
00078 CalculateData::~CalculateData() {
00079 
00080 }
00081 
00082 
00083 
00084 
00085 void CalculateData::enable(){
00086     //Anfangsbedingungen auf NULL
00087     for (int i = 0; i<3; i++){
00088         acc[i]=0;
00089         acc_old[i]=0;
00090         speed[i]=0;
00091         speed_old[i]=0;
00092         pos[i]=0;
00093     }
00094     
00095     t_old = 0;
00096     counter=0;
00097     
00098     //Timer und Thread starten
00099     timer.start();
00100     periodic_task=true;
00101     thread.start(callback(this, &CalculateData::run));
00102     thread.set_priority(osPriorityHigh);
00103 }
00104 
00105 //Timer und Thread beenden
00106 void CalculateData::disable(){
00107     periodic_task=false;
00108     timer.stop();
00109     thread.join();
00110 }
00111 
00112 
00113 
00114 
00115 
00116 //Periodischer Task
00117 void CalculateData::run() {
00118     
00119     int buf_acc[3];
00120     int buf_gyro[3];
00121     int buf_mag[3];
00122     
00123     while(periodic_task){
00124         
00125         counter +=1;
00126         
00127         //Sensoren und Timer auslesen
00128         //accelerometer->get_x_axes(buf_acc);
00129         acc_gyro->get_x_axes(buf_acc);
00130         acc_gyro->get_g_axes(buf_gyro);
00131         magnetometer->get_m_axes(buf_mag);
00132         t[counter-1]=(float)timer.read_ms();
00133         
00134         for(int i=0;i<3;i++){
00135             array_acc[i][counter-1]=buf_acc[i];
00136             array_gyro[i][counter-1]=buf_gyro[i];
00137             array_mag[i][counter-1]=buf_mag[i];
00138         }
00139 
00140         Thread::wait(2);
00141     }
00142 }
00143 
00144 
00145 
00146 void CalculateData::getValue(Liste *list){
00147     //udates the Values
00148     update();
00149     
00150     //save Data in List
00151     list->accX= acc[0];
00152     list->accY= acc[1];
00153     list->accZ= acc[2];
00154     list->speedX= speed[0];
00155     list->speedY= speed[1];
00156     list->speedZ= speed[2];
00157     list->posX= pos[0];
00158     list->posY= pos[1];
00159     list->posZ= pos[2];
00160     list->time= t_old;
00161 }
00162 
00163 
00164 //calculate actual Values
00165 void CalculateData::update(){
00166     
00167     for(int i=0;i<counter;i++){
00168         
00169         float buff[3];
00170         
00171         calibrationMag(array_mag[0][i],array_mag[1][i],array_mag[2][i], buff);
00172                 
00173         mahony->update(-array_gyro[2][i]*PI/180000.0f, array_gyro[0][i]*PI/180000.0f, array_gyro[1][i]*PI/180000.0f,
00174         -array_acc[2][i]/1000.0f, array_acc[0][i]/1000.0f, array_acc[1][i]/1000.0f,
00175         buff[2], buff[0], -buff[1]);
00176         
00177         mahony->getEuler();
00178         transform(array_acc, i, acc, mahony->getRoll(), mahony->getPitch(), mahony->getYaw());
00179         
00180         filterAcc(acc);
00181         
00182         integrate(acc, acc_old, speed, t[i], t_old); //in mm/s
00183         
00184         //filterSpeed(speed);
00185         
00186         integrate(speed, speed_old, pos, t[i], t_old); //in um
00187         
00188         for (int j=0; j<3;j++){
00189             acc_old[j] = acc[j];
00190             speed_old[j] = speed[j];
00191         }
00192         t_old=t[i];
00193     }
00194     counter = 0;
00195     printf("Roll:\t%i\tPitch:\t%i\tYaw:\t%i\r\n",mahony->getRoll(),mahony->getPitch(),mahony->getYaw());
00196     //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);
00197     //printf("%f, %f, %f\r\n",acc[0],acc[1],acc[2]);
00198 }
00199 
00200 
00201 
00202 
00203 
00204 
00205 //Daten Integrieren nach expl. Euler
00206 void CalculateData::integrate(float *x, float *x_old, float *y, float t, float t_old){
00207     float dt = t-t_old;
00208     
00209     for(int i = 0;i < 3;i++){
00210         y[i] = y[i] + ((x[i]+x_old[i])/2.0f)*dt;
00211     }
00212         
00213 }
00214 
00215 
00216 
00217 
00218 //FIR-Filter (Accelerometer)
00219 void CalculateData::filterAcc(float *array){
00220     
00221     double sum_array[] = {0, 0, 0};
00222     
00223     for(int j=0;j<3;j++){
00224         for(int i=0;i<FILTERSIZE-2;i++){
00225             acc_filter[j][i] = acc_filter[j][i+1];
00226             sum_array[j] += (filter_koef[i] * (float)acc_filter[j][i]);
00227         }
00228         acc_filter[j][FILTERSIZE-1] = array[j];
00229         sum_array[j] += (filter_koef[FILTERSIZE-1] * (float)acc_filter[j][FILTERSIZE-1]);
00230         array[j] = sum_array[j]/FILTERSIZE;
00231     }        
00232 
00233 }
00234 
00235 
00236 //FIR-Filter (Speed)
00237 void CalculateData::filterSpeed(float *array){
00238     double sum_array[] = {0, 0, 0};
00239     
00240     for(int j=0;j<3;j++){
00241         for(int i=0;i<FILTERSIZE-2;i++){
00242             speed_filter[j][i] = speed_filter[j][i+1];
00243             sum_array[j] += (filter_koef[i] * (float)speed_filter[j][i]);
00244         }
00245         speed_filter[j][FILTERSIZE-1] = array[j];
00246         sum_array[j] += (filter_koef[FILTERSIZE-1] * (float)speed_filter[j][FILTERSIZE-1]);
00247         array[j] = sum_array[j]/FILTERSIZE;
00248     }
00249 }
00250 
00251 
00252 void CalculateData::calibrationMag(int x,int y,int z, float *buff){
00253     buff[0] = magtransformationmatrix[0][0]*((float)x-bias[0])+
00254     magtransformationmatrix[0][1]*((float)y-bias[1])+
00255     magtransformationmatrix[0][2]*((float)z-bias[2]);
00256     
00257     buff[1] = magtransformationmatrix[1][0]*((float)x-bias[0])+
00258     magtransformationmatrix[1][1]*((float)y-bias[1])+
00259     magtransformationmatrix[1][2]*((float)z-bias[2]);
00260     
00261     buff[2] = magtransformationmatrix[2][0]*((float)x-bias[0])+
00262     magtransformationmatrix[2][1]*((float)y-bias[1])+
00263     magtransformationmatrix[2][2]*((float)z-bias[2]);
00264 }
00265 
00266 
00267 //Beschleunigung in globale Koordinaten Transformieren
00268 void CalculateData::transform(int old_val[3][100], int index, float *new_val, int roll, int pitch, int yaw){
00269     
00270     //Koeffizienten zum umrechnen
00271     float sa = sin((float)yaw*PI/180.0f);    
00272     float ca = cos((float)yaw*PI/180.0f);
00273     float sb = sin((float)(-pitch)*PI/180.0f);
00274     float cb = cos((float)(-pitch)*PI/180.0f);
00275     float sc = sin((float)roll*PI/180.0f);
00276     float cc = cos((float)roll*PI/180.0f);
00277     
00278     //Transformation
00279     /*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);
00280     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);
00281     new_val[2] = -(float)old_val[2][index]*(-sb) - (float)old_val[0][index]*(cb*sc) + (float)old_val[1][index]*(cb*cc);
00282     */
00283     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];
00284     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];
00285     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];
00286 }