Cyrill Zoller
calculate
Dependencies: mbed X_NUCLEO_IKS01A3 Mahony_Algorithm
Diff: CalculateData.cpp
- Revision:
- 2:4cccdc792719
- Parent:
- 1:48e219526d0f
- Child:
- 3:795998b31c32
diff -r 48e219526d0f -r 4cccdc792719 CalculateData.cpp
--- a/CalculateData.cpp Mon Apr 13 09:37:17 2020 +0000
+++ b/CalculateData.cpp Wed Apr 15 12:13:15 2020 +0000
@@ -31,13 +31,22 @@
using namespace std;
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
+const int FILTERSIZE = 28;
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 int speed_filter[3][FILTERSIZE];
+const double filter_koef[FILTERSIZE] = {0.04002232346037, -0.1345598020407, -0.05881026752833, -0.03718019471321,
+ -0.02581625686635, -0.01315040166028, 0.003361386671892, 0.0234601656009,
+ 0.04581625763125, 0.06897560043795, 0.0905455051097, 0.1088826077838,
+ 0.1221264326899, 0.1290832392388, 0.1290832392388, 0.1221264326899,
+ 0.1088826077838, 0.0905455051097, 0.06897560043795, 0.04581625763125,
+ 0.0234601656009, 0.003361386671892, -0.01315040166028, -0.02581625686635,
+ -0.03718019471321, -0.05881026752833, -0.1345598020407, 0.04002232346037
+ };
+
@@ -66,23 +75,6 @@
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;
@@ -100,12 +92,12 @@
//Timer und Ticker starten
timer.start();
- //ticker.attach(callback(this, &CalculateData::run), PERIOD);
+ ticker.attach(callback(this, &CalculateData::run), PERIOD);
}
//Timer und Ticker beenden
void CalculateData::disable(){
- ticker.detach();
+ //ticker.detach();
timer.stop();
}
@@ -128,10 +120,12 @@
acc_gyro->get_g_axes(gyro);
t=timer.read_ms();
- filter(acc, gyro); //Daten Filtern
+ 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
+ //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
}
@@ -141,50 +135,58 @@
int dt = t-t_old;
for(int i = 0;i < 3;i++){
- y[i] = y[i] + ((x[i] + x_old[i])/2)*dt;
+ y[i] = (uint32_t)((double)y[i] + ((double)(x[i] + (double)x_old[i])/2.0)*dt/1000.0);
}
}
-//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};
+void CalculateData::filter(int *array, int type){
+
+ double sum_array[] = {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];
+ 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;
- 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]);
+ 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;
- acc[j] = (int)sum_acc[j];
- gyro[j] = (int)sum_gyro[j];
- }
+ 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;
+ }
}
@@ -222,4 +224,24 @@
for(int i=0;i<3;i++){
array[i] = angle[i];
}
-}
\ No newline at end of file
+}
+
+
+/*
+//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];
+ }
+}
+*/
\ No newline at end of file