Cyrill Zoller
calculate
Dependencies: mbed X_NUCLEO_IKS01A3 Mahony_Algorithm
Diff: CalculateData.cpp
- Revision:
- 3:795998b31c32
- Parent:
- 2:4cccdc792719
- Child:
- 4:7d13076ecece
--- 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