Diff: CalculateData.cpp

Revision:

4:7d13076ecece

Parent:

3:795998b31c32

Child:

5:62994bb9fff9

diff -r 795998b31c32 -r 7d13076ecece CalculateData.cpp
--- 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());
+}