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