Revision 4:7d13076ecece, committed 2020-04-24

Comitter:

zollecy1

Date:

Fri Apr 24 15:10:06 2020 +0000

Parent:

3:795998b31c32

Child:

5:62994bb9fff9

Commit message:

.

Changed in this revision

--- a/CalculateData.cpp	Thu Apr 16 19:34:49 2020 +0000
+++ b/CalculateData.cpp	Fri Apr 24 15:10:06 2020 +0000
@@ -29,12 +29,14 @@
 #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 int                   FILTERSIZE = 28;
 static int                  acc_filter[3][FILTERSIZE];
 static int                  gyro_filter[3][FILTERSIZE];
@@ -59,11 +61,16 @@
     /* 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_x();
     acc_gyro->enable_g();
+    magnetometer->enable();
+    
+    //initialisation Mahony
+    mahony = new MahonyAHRS(166.7);
 }
 
 
@@ -110,45 +117,37 @@
 
 //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);
+        //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]=(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];
+            array_mag[i][counter-1]=buf_mag[i];
         }
 
-        Thread::wait(10);
+        Thread::wait(2);
     }
 }
 
 
 
 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;
+    //udates the Values
+    update();
     
     //save Data in List
     list->accX= acc[0];
@@ -170,7 +169,30 @@
 }
 
 
-
+//calculate actual Values
+void CalculateData::update(){
+    
+    for(int i=0;i<counter;i++){
+        mahony->update(array_gyro[0][i]*PI/180000.0f, array_gyro[1][i]*PI/180000.0f, array_gyro[2][i]*PI/180000.0f, array_acc[0][i]/1000.0f, array_acc[1][i]/1000.0f, array_acc[2][i]/1000.0f,
+        0.0f,0.0f,0.0f);// array_mag[0][i]/1000.0f, array_mag[1][i]/1000.0f, array_mag[2][i]/1000.0f);
+        /*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;
+    mahony->getEuler();
+    printf("Roll:\t%i\tPitch\t%i\tYaw:\t%i\r\n",mahony->getRoll(),mahony->getPitch(),mahony->getYaw());
+}
 
 
 

--- a/CalculateData.h	Thu Apr 16 19:34:49 2020 +0000
+++ b/CalculateData.h	Fri Apr 24 15:10:06 2020 +0000
@@ -29,6 +29,7 @@
 #include "mbed.h"
 #include "XNucleoIKS01A3.h"
 #include "Liste.h"
+#include "MahonyAHRS.h"
 
 class CalculateData {
 
@@ -41,6 +42,7 @@
     void                        enable();
     void                        disable();
     void                        getValue(Liste *list);
+    void                        update();
     
     
 
@@ -58,15 +60,15 @@
     
     Thread                      thread;
     Timer                       timer;
+    MahonyAHRS                  *mahony;
     
-    int                         array_acc[3][30];       //Matrix periodiv Sensor Data
-    int                         array_gyro[3][30];      //Matrix periodiv Sensor Data
+    int                         array_acc[3][100];       //Matrix periodic Sensor Data
+    int                         array_gyro[3][100];      //Matrix periodic Sensor Data
+    int                         array_mag[3][100];      //Matrix periodic 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];

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mahony_Algorithm.lib	Fri Apr 24 15:10:06 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/b50559/code/Mahony_Algorithm/#da9dac34fd93

--- a/main.cpp	Thu Apr 16 19:34:49 2020 +0000
+++ b/main.cpp	Fri Apr 24 15:10:06 2020 +0000
@@ -43,14 +43,14 @@
     Liste myList;
     calculate.enable();
     while(1){    
-        
+        /*
         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");
-        
+        */calculate.update();
         Thread::wait(100);
     }
 }