Revision 1:48e219526d0f, committed 2020-04-13

Comitter:

zollecy1

Date:

Mon Apr 13 09:37:17 2020 +0000

Parent:

0:313fbc3a198a

Child:

2:4cccdc792719

Commit message:

.;

Changed in this revision

--- a/CalculateData.cpp	Wed Apr 01 16:47:06 2020 +0000
+++ b/CalculateData.cpp	Mon Apr 13 09:37:17 2020 +0000
@@ -22,78 +22,39 @@
 |**********************************************************************;
 **/
 
+#define  PI (3.14159265)
 #include "CalculateData.h"
 #include "mbed.h"
 #include "XNucleoIKS01A3.h"
+#include <math.h>
 
 using namespace std;
 
-const float             CalculateData::PERIOD = 0.01f; //Periode von 10 ms
-
+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};
 
-static XNucleoIKS01A3   *mems_expansion_board;
-static LIS2MDLSensor    *magnetometer;
-static HTS221Sensor     *hum_temp;
-static LPS22HHSensor    *press_temp;
-static LSM6DSOSensor    *acc_gyro;
-static LIS2DW12Sensor   *accelerometer;
-static STTS751Sensor    *temp;
-float                   value1, value2;
-int32_t                 axes[3];
 
 
 
 CalculateData::CalculateData(PinName p0, PinName p1, PinName p2, PinName p3,
-                            PinName p4, PinName p5, PinName p6){
-    
+                            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 */
-    magnetometer = mems_expansion_board->magnetometer;
-    hum_temp = mems_expansion_board->ht_sensor;
-    press_temp = mems_expansion_board->pt_sensor;
     acc_gyro = mems_expansion_board->acc_gyro;
     accelerometer = mems_expansion_board->accelerometer;
-    temp = mems_expansion_board->t_sensor;                          
-   
+    
     /* Enable all sensors */
-    hum_temp->enable();
-    press_temp->enable();
-    temp->enable();
-    magnetometer->enable();
     accelerometer->enable_x();
-    acc_gyro->enable_x();
+    //acc_gyro->enable_x();
     acc_gyro->enable_g();
-    
-    /*while(1){
-        printf("\r\n");
-
-        hum_temp->get_temperature(&value1);
-        hum_temp->get_humidity(&value2);
-        printf("HTS221: [temp] %4.2f C,   [hum] %4.2f%%\r\n", value1, value2);
-
-        press_temp->get_temperature(&value1);
-        press_temp->get_pressure(&value2);
-        printf("LPS22HH: [temp] %4.2f C, [press] %6.2f mbar\r\n", value1, value2);
-
-        temp->get_temperature(&value1);
-        printf("STTS751: [temp] %4.2f C\r\n", value1);
-
-        printf("---\r\n");
-
-        magnetometer->get_m_axes(axes);
-        printf("LIS2MDL [mag/mgauss]:  %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
-
-        accelerometer->get_x_axes(axes);
-        printf("LIS2DW12 [acc/mg]:  %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
-
-        acc_gyro->get_x_axes(axes);
-        printf("LSM6DSO [acc/mg]:      %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
-
-        acc_gyro->get_g_axes(axes);
-        printf("LSM6DSO [gyro/mdps]:   %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
-    }*/
 }
 
 
@@ -102,62 +63,163 @@
 }
 
 
-void CalculateData::enable(){
-    //Starten des periodischen Task
-    ticker.attach(callback(this, &CalculateData::run), PERIOD);
+
 
-     
-}
-
-void CalculateData::disable(){
-    ticker.detach(); // Stoppt den periodischen Task    
+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);
 }
 
-void CalculateData::getData(){
-        printf("\r\n");
-
-        hum_temp->get_temperature(&value1);
-        hum_temp->get_humidity(&value2);
-        printf("HTS221: [temp] %4.2f C,   [hum] %4.2f%%\r\n", value1, value2);
-
-        press_temp->get_temperature(&value1);
-        press_temp->get_pressure(&value2);
-        printf("LPS22HH: [temp] %4.2f C, [press] %6.2f mbar\r\n", value1, value2);
-
-        temp->get_temperature(&value1);
-        printf("STTS751: [temp] %4.2f C\r\n", value1);
-
-        printf("---\r\n");
-
-        magnetometer->get_m_axes(axes);
-        printf("LIS2MDL [mag/mgauss]:  %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
-
-        accelerometer->get_x_axes(axes);
-        printf("LIS2DW12 [acc/mg]:  %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
-
-        acc_gyro->get_x_axes(axes);
-        printf("LSM6DSO [acc/mg]:      %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
-
-        acc_gyro->get_g_axes(axes);
-        printf("LSM6DSO [gyro/mdps]:   %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
-
-}
-
-
-//Periodischer Task
-void CalculateData::run() {
-    
-        getData();    
-    
+//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];
+    } 
+}
\ No newline at end of file

--- a/CalculateData.h	Wed Apr 01 16:47:06 2020 +0000
+++ b/CalculateData.h	Mon Apr 13 09:37:17 2020 +0000
@@ -39,15 +39,42 @@
     virtual                     ~CalculateData();                
     void                        enable();
     void                        disable();
-    void                        getData();
+    void                        run();
+    void                        getAccelerometer(int *array);
+    void                        getSpeed(int *array);
+    void                        getPosition(int *array);
+    void                        getGyro(int *array);
+    void                        getAngle(int *array);
+    
+    
+    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;
+    int32_t                     acc_err[3];
+    int32_t                     gyro_err[3];
 
 private:
 
     static const float          PERIOD;
     
-    Ticker                      ticker;   
+    //void                        run();
+    void                        integrate(int *x, int *x_old, int *y, int t, int t_old);
+    void                        transform(int *acc, int *angle);
+    void                        filter(int *acc, int *gyro);
+
     
-    void                        run();
+    
+    Ticker                      ticker;
+    Timer                       timer;  
+    
+  
     
 };
 

--- a/main.cpp	Wed Apr 01 16:47:06 2020 +0000
+++ b/main.cpp	Mon Apr 13 09:37:17 2020 +0000
@@ -28,22 +28,31 @@
 
 //initialise DigitalIO
 DigitalOut myled(LED1);
-DigitalOut enable(PB_15);
-DigitalIn leftsensor(PA_0);
 DigitalIn user_button(USER_BUTTON);
 
 
 //Generate object
 CalculateData calculate(D14, D15, D4, D5, A3, D6, A4);
 
+int pos[3];
+int speed[3];
+int acc[3];
 
 int main(){
-    wait(4);
     calculate.enable();
-    wait(8);
-    calculate.disable();
-    
-    
+    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%5.2f\t%5.2f\t%5.2f\r\n",(speed[0]*9.81)/1000, (speed[1]*9.81)/1000, (speed[2]*9.81)/1000);
+        printf("pos: \t%5.2f\t%5.2f\t%5.2f\r\n",(pos[0]*9.81)/1000, (pos[1]*9.81)/1000, (pos[2]*9.81)/1000);
+        printf("\r\n----------\r\n\n");
+        wait_ms(500);
+    }
 }