Revision 3:795998b31c32, committed 2020-04-16

Comitter:

zollecy1

Date:

Thu Apr 16 19:34:49 2020 +0000

Parent:

2:4cccdc792719

Child:

4:7d13076ecece

Commit message:

.

Changed in this revision

--- a/CalculateData.cpp	Wed Apr 15 12:13:15 2020 +0000
+++ b/CalculateData.cpp	Thu Apr 16 19:34:49 2020 +0000
@@ -27,10 +27,11 @@
 #include "mbed.h"
 #include "XNucleoIKS01A3.h"
 #include <math.h>
+#include <Thread.h>
+#include "Liste.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
@@ -50,7 +51,6 @@
 
 
 
-
 CalculateData::CalculateData(PinName p0, PinName p1, PinName p2, PinName p3,
                             PinName p4, PinName p5, PinName p6){                           
     /* Instantiate the expansion board */
@@ -84,21 +84,24 @@
         speed[i]=0;
         speed_old[i]=0;
         angle[i]=0;
-        pos[i]=0; 
+        pos[i]=0;
     }
     
-    t = 0;
     t_old = 0;
+    counter=0;
     
-    //Timer und Ticker starten
+    //Timer und Thread starten
     timer.start();
-    ticker.attach(callback(this, &CalculateData::run), PERIOD);
+    periodic_task=true;
+    thread.start(callback(this, &CalculateData::run));
+    thread.set_priority(osPriorityHigh);
 }
 
-//Timer und Ticker beenden
+//Timer und Thread beenden
 void CalculateData::disable(){
-    //ticker.detach();
-    timer.stop();    
+    periodic_task=false;
+    timer.stop();
+    thread.join();
 }
 
 
@@ -107,35 +110,79 @@
 
 //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];
+    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);
     }
-    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
 }
 
 
+
+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(int *x, int *x_old, int *y, int t, int t_old){
-    int dt = t-t_old;
+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] = (uint32_t)((double)y[i] + ((double)(x[i] + (double)x_old[i])/2.0)*dt/1000.0);
+        y[i] = y[i] + ((x[i]+x_old[i])/2.0f)*dt;
     }
         
 }
@@ -143,89 +190,54 @@
 
 
 
-//Filtern der Daten mittels FIR-Filter
-void CalculateData::filter(int *array, int type){
+//FIR-Filter (Accelerometer)
+void CalculateData::filterAcc(int array[3][30],int index, double *target){
     
     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;  
+    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];
     }
 }
 
-
-
-
-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];
-    } 
+//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];
+    }
 }
 
 
-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

--- a/CalculateData.h	Wed Apr 15 12:13:15 2020 +0000
+++ b/CalculateData.h	Thu Apr 16 19:34:49 2020 +0000
@@ -28,6 +28,7 @@
 
 #include "mbed.h"
 #include "XNucleoIKS01A3.h"
+#include "Liste.h"
 
 class CalculateData {
 
@@ -39,38 +40,43 @@
     virtual                     ~CalculateData();                
     void                        enable();
     void                        disable();
-    void                        run();
-    void                        getAccelerometer(int *array);
-    void                        getSpeed(int *array);
-    void                        getPosition(int *array);
-    void                        getGyro(int *array);
-    void                        getAngle(int *array);
+    void                        getValue(Liste *list);
     
     
-    int32_t                     acc[3];
-    int32_t                     acc_old[3];
-    int32_t                     gyro[3];
-    int32_t                     gyro_old[3];
-    int32_t                     speed[3];
-    int32_t                     speed_old[3];
-    int32_t                     angle[3];
-    int32_t                     pos[3];
-    int32_t                     t;
-    int32_t                     t_old;
 
 private:
 
-    static const float          PERIOD;
     
-    void                        integrate(int *x, int *x_old, int *y, int t, int t_old);
+    void                        integrate(double *x, double *x_old, double *y, double t, double t_old);
     //void                        transform(int *acc, int *angle);
-    void                        filter(int *array, int type);
+    void                        filterAcc(int array[3][30],int index, double *target);
+    void                        filterGyro(int array[3][30],int index, double *target);
+    void                        filterSpeed(double *array, double *target);
+    void                        run();
 
     
     
-    Ticker                      ticker;
-    Timer                       timer;  
+    Thread                      thread;
+    Timer                       timer;
+    
+    int                         array_acc[3][30];       //Matrix periodiv Sensor Data
+    int                         array_gyro[3][30];      //Matrix periodiv Sensor Data
+    int                         counter;                //count Matrix records
+    bool                        periodic_task;          //boolean periodic condition
+    
     
+    int                         buf_acc[3];         
+    int                         buf_gyro[3];
+    double                      acc[3];         
+    double                      gyro[3];
+    double                      acc_old[3];
+    double                      gyro_old[3];
+    double                      speed[3];
+    double                      speed_old[3];
+    double                      angle[3];
+    double                      pos[3];
+    double                      t[30];
+    double                      t_old;
   
     
 };

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Liste.h	Thu Apr 16 19:34:49 2020 +0000
@@ -0,0 +1,29 @@
+#ifndef LISTE_H
+#define LISTE_H
+
+//testListe
+struct Liste {
+    int time;
+    
+    float accX;
+    float accY;
+    float accZ;
+    
+    float speedX;
+    float speedY;
+    float speedZ;
+    
+    float posX;
+    float posY;
+    float posZ;
+    
+    float gyroX;
+    float gyroY;
+    float gyroZ;
+    
+    float angleX;
+    float angleY;
+    float angleZ;
+};
+
+#endif
\ No newline at end of file

--- a/main.cpp	Wed Apr 15 12:13:15 2020 +0000
+++ b/main.cpp	Thu Apr 16 19:34:49 2020 +0000
@@ -24,6 +24,7 @@
 
 #include "mbed.h"
 #include "CalculateData.h"
+#include "Liste.h"
 
 
 //initialise DigitalIO
@@ -34,24 +35,23 @@
 //Generate object
 CalculateData calculate(D14, D15, D4, D5, A3, D6, A4);
 
-int pos[3];
-int speed[3];
-int acc[3];
+
+
+
 
 int main(){
+    Liste myList;
     calculate.enable();
     while(1){    
-        calculate.run();
-        calculate.getAccelerometer(acc);
-        calculate.getSpeed(speed);
-        calculate.getPosition(pos);
         
-        printf("\r\n----------\r\n\n");
-        printf("Accelerometer: \t%5.2f\t%5.2f\t%5.2f\r\n",(double)acc[0], (double)acc[1], (double)acc[2]);
-        printf("Speed: \t\t%5.2f\t%5.2f\t%5.2f\r\n",(double)speed[0], (double)speed[1], (double)speed[2]);
-        printf("pos: \t\t%5.2f\t%5.2f\t%5.2f\r\n%i",(double)pos[0], (double)pos[1], (double)pos[2], calculate.t);
-        printf("\r\n----------\r\n\n");
-        wait_ms(1000);
+        calculate.getValue(&myList);
+        printf("\t\tX\t\tY\t\tZ\r\n");
+        printf("ACC:\t\t%f\t%f\t%f\r\n", myList.accX, myList.accY, myList.accZ);
+        printf("SPEED:\t\t%f\t%f\t%f\r\n", myList.speedX, myList.speedY, myList.speedZ);
+        printf("POS:\t\t%f\t%f\t%f\r\n", myList.posX, myList.posY, myList.posZ);
+        printf("\r\n\r\n-------------------------\r\n\r\n\r\n");
+        
+        Thread::wait(100);
     }
 }
 

--- a/mbed-os.lib	Wed Apr 15 12:13:15 2020 +0000
+++ b/mbed-os.lib	Thu Apr 16 19:34:49 2020 +0000
@@ -1,1 +1,1 @@
-https://github.com/armmbed/mbed-os/#73f096399b4cda1f780b140c87afad9446047432
+https://github.com/ARMmbed/mbed-os/#565ab149819481224ab43f878c3921b14b11d180

--- a/mbed_app.json	Wed Apr 15 12:13:15 2020 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,3 +0,0 @@
-{
-    "requires": ["bare-metal"]
-}
\ No newline at end of file