Cyrill Zoller
calculate
Dependencies: mbed X_NUCLEO_IKS01A3 Mahony_Algorithm
Diff: CalculateData.cpp
- Revision:
- 5:62994bb9fff9
- Parent:
- 4:7d13076ecece
- Child:
- 6:4641f2b2684b
--- a/CalculateData.cpp Fri Apr 24 15:10:06 2020 +0000
+++ b/CalculateData.cpp Wed Apr 29 10:54:59 2020 +0000
@@ -37,19 +37,20 @@
static LSM6DSOSensor *acc_gyro; //Gyro-Sensor
static LIS2DW12Sensor *accelerometer; //Acceleroation-Sensor
static LIS2MDLSensor *magnetometer; //Magnetometer
+const float accOffset[3] = {-0.007734011220907f, -0.007085474965694f, -9.910594323583762f};
const int FILTERSIZE = 28;
-static int acc_filter[3][FILTERSIZE];
-static int gyro_filter[3][FILTERSIZE];
-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
- };
-
+static float acc_filter[3][FILTERSIZE];
+static float gyro_filter[3][FILTERSIZE];
+static float speed_filter[3][FILTERSIZE];
+float magtransformationmatrix[3][3]={{1.624f, 0.049f, 0.011f}, {-0.004f, 1.668f, 0.001f}, {0.074f, 0.001f, 1.635f}};
+float bias[3]={-466.649f, -273.636f, 58.044f};
+const float filter_koef[FILTERSIZE] = {1.54459639027449f, -5.19311640449656f, -2.26968649197199f, -1.43490906020467f,
+ -0.996336387257677f, -0.507518334242666f, 0.129727243968288f, 0.905406882193463f,
+ 1.76820384311742f, 2.66200095951371f, 3.49445629978250f, 4.20214691216170f,
+ 4.71327076441993f, 4.98175738274206f, 4.98175738274206f, 4.71327076441993f,
+ 4.20214691216170f, 3.49445629978250f, 2.66200095951371f, 1.76820384311742f,
+ 0.905406882193463f, 0.129727243968288f, -0.507518334242666f, -0.996336387257677f,
+ -1.43490906020467f, -2.26968649197199f, -5.19311640449656f, 1.54459639027449f};
@@ -70,7 +71,7 @@
magnetometer->enable();
//initialisation Mahony
- mahony = new MahonyAHRS(166.7);
+ mahony = new MahonyAHRS(200.0);
}
@@ -86,11 +87,8 @@
for (int i = 0; i<3; i++){
acc[i]=0;
acc_old[i]=0;
- gyro[i]=0;
- gyro_old[i]=0;
speed[i]=0;
speed_old[i]=0;
- angle[i]=0;
pos[i]=0;
}
@@ -131,7 +129,7 @@
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;
+ t[counter-1]=(float)timer.read_ms();
for(int i=0;i<3;i++){
array_acc[i][counter-1]=buf_acc[i];
@@ -159,12 +157,6 @@
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;
}
@@ -173,25 +165,36 @@
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);
+
+ float buff[3];
+
+ calibrationMag(array_mag[0][i],array_mag[1][i],array_mag[2][i], buff);
+
+ mahony->update(-array_gyro[2][i]*PI/180000.0f, array_gyro[0][i]*PI/180000.0f, array_gyro[1][i]*PI/180000.0f,
+ -array_acc[2][i]/1000.0f, array_acc[0][i]/1000.0f, array_acc[1][i]/1000.0f,
+ buff[2], buff[0], -buff[1]);
+
+ mahony->getEuler();
+ transform(array_acc, i, acc, mahony->getRoll(), mahony->getPitch(), mahony->getYaw());
+
+ //filterAcc(acc);
+
+ integrate(acc, acc_old, speed, t[i], t_old); //in mm/s
+
+ //filterSpeed(speed);
+
+ integrate(speed, speed_old, pos, t[i], t_old); //in um
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];*/
+ 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());
+ //printf("Roll:\t%i\tPitch:\t%i\tYaw:\t%i\r\n",mahony->getRoll(),mahony->getPitch(),mahony->getYaw());
+ printf("%f, %f, %f, %f, %f, %f, %f\r\n",acc[0],acc[1],acc[2], t_old/1000.0f, speed[0]/1000.0f,speed[1]/1000.0f,speed[2]/1000.0f);
+ //printf("%f, %f, %f\r\n",acc[0],acc[1],acc[2]);
}
@@ -200,8 +203,8 @@
//Daten Integrieren nach expl. Euler
-void CalculateData::integrate(double *x, double *x_old, double *y, double t, double t_old){
- double dt = t-t_old;
+void CalculateData::integrate(float *x, float *x_old, float *y, float t, float t_old){
+ float dt = t-t_old;
for(int i = 0;i < 3;i++){
y[i] = y[i] + ((x[i]+x_old[i])/2.0f)*dt;
@@ -213,69 +216,71 @@
//FIR-Filter (Accelerometer)
-void CalculateData::filterAcc(int array[3][30],int index, double *target){
+void CalculateData::filterAcc(float *array){
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];
- sum_array[j] += (filter_koef[i] * (double)acc_filter[j][i]);
+ sum_array[j] += (filter_koef[i] * (float)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];
+ acc_filter[j][FILTERSIZE-1] = array[j];
+ sum_array[j] += (filter_koef[FILTERSIZE-1] * (float)acc_filter[j][FILTERSIZE-1]);
+ array[j] = sum_array[j]/FILTERSIZE;
}
}
-//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];
- }
-}
//FIR-Filter (Speed)
-void CalculateData::filterSpeed(double *array, double *target){
+void CalculateData::filterSpeed(float *array){
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]);
+ sum_array[j] += (filter_koef[i] * (float)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];
+ sum_array[j] += (filter_koef[FILTERSIZE-1] * (float)speed_filter[j][FILTERSIZE-1]);
+ array[j] = sum_array[j]/FILTERSIZE;
}
}
+void CalculateData::calibrationMag(int x,int y,int z, float *buff){
+ buff[0] = magtransformationmatrix[0][0]*((float)x-bias[0])+
+ magtransformationmatrix[0][1]*((float)y-bias[1])+
+ magtransformationmatrix[0][2]*((float)z-bias[2]);
+
+ buff[1] = magtransformationmatrix[1][0]*((float)x-bias[0])+
+ magtransformationmatrix[1][1]*((float)y-bias[1])+
+ magtransformationmatrix[1][2]*((float)z-bias[2]);
+
+ buff[2] = magtransformationmatrix[2][0]*((float)x-bias[0])+
+ magtransformationmatrix[2][1]*((float)y-bias[1])+
+ magtransformationmatrix[2][2]*((float)z-bias[2]);
+}
-/*
+
//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
+void CalculateData::transform(int old_val[3][100], int index, float *new_val, int roll, int pitch, int yaw){
+
+ //Koeffizienten zum umrechnen
+ float sa = sin((float)yaw*PI/180.0f);
+ float ca = cos((float)yaw*PI/180.0f);
+ float sb = sin((float)(-pitch)*PI/180.0f);
+ float cb = cos((float)(-pitch)*PI/180.0f);
+ float sc = sin((float)roll*PI/180.0f);
+ float cc = cos((float)roll*PI/180.0f);
//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];
- }
+ /*new_val[0] = -(float)old_val[2][index]*(ca*cb) - (float)old_val[0][index]*(ca*sb*sc-cc*sa) + (float)old_val[1][index]*(ca*cc*sb+sa*sc);
+ new_val[1] = -(float)old_val[2][index]*(cb*sa) - (float)old_val[0][index]*(ca*cc+sa*sb*sc) + (float)old_val[1][index]*(cc*sa*sb-ca*sc);
+ new_val[2] = -(float)old_val[2][index]*(-sb) - (float)old_val[0][index]*(cb*sc) + (float)old_val[1][index]*(cb*cc);
+ */
+ new_val[0] = (-(float)old_val[2][index]*(ca*cb) + (float)old_val[0][index]*(cb*sa) + (float)old_val[1][index]*(-sb))*0.0098f - accOffset[0];
+ new_val[1] = (-(float)old_val[2][index]*(ca*sb*sc-cc*sa) + (float)old_val[0][index]*(ca*cc+sa*sb*sc) + (float)old_val[1][index]*(cb*sc))*0.0098f - accOffset[1];
+ new_val[2] = (-(float)old_val[2][index]*(ca*cc*sb+sa*sc) + (float)old_val[0][index]*(cc*sa*sb-ca*sc) + (float)old_val[1][index]*(cb*cc))*0.0098f - accOffset[2];
}
-*/
\ No newline at end of file