calculate
Dependencies: mbed X_NUCLEO_IKS01A3 Mahony_Algorithm
Diff: CalculateData.cpp
- Revision:
- 1:48e219526d0f
- Parent:
- 0:313fbc3a198a
- Child:
- 2:4cccdc792719
--- a/CalculateData.cpp Wed Apr 01 16:47:06 2020 +0000 +++ b/CalculateData.cpp Mon Apr 13 09:37:17 2020 +0000 @@ -22,78 +22,39 @@ |**********************************************************************; **/ +#define PI (3.14159265) #include "CalculateData.h" #include "mbed.h" #include "XNucleoIKS01A3.h" +#include <math.h> using namespace std; -const float CalculateData::PERIOD = 0.01f; //Periode von 10 ms - +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 +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 XNucleoIKS01A3 *mems_expansion_board; -static LIS2MDLSensor *magnetometer; -static HTS221Sensor *hum_temp; -static LPS22HHSensor *press_temp; -static LSM6DSOSensor *acc_gyro; -static LIS2DW12Sensor *accelerometer; -static STTS751Sensor *temp; -float value1, value2; -int32_t axes[3]; CalculateData::CalculateData(PinName p0, PinName p1, PinName p2, PinName p3, - PinName p4, PinName p5, PinName p6){ - + PinName p4, PinName p5, PinName p6){ /* Instantiate the expansion board */ mems_expansion_board = XNucleoIKS01A3::instance(p0, p1, p2, p3, p4, p5, p6); /* Retrieve the composing elements of the expansion board */ - magnetometer = mems_expansion_board->magnetometer; - hum_temp = mems_expansion_board->ht_sensor; - press_temp = mems_expansion_board->pt_sensor; acc_gyro = mems_expansion_board->acc_gyro; accelerometer = mems_expansion_board->accelerometer; - temp = mems_expansion_board->t_sensor; - + /* Enable all sensors */ - hum_temp->enable(); - press_temp->enable(); - temp->enable(); - magnetometer->enable(); accelerometer->enable_x(); - acc_gyro->enable_x(); + //acc_gyro->enable_x(); acc_gyro->enable_g(); - - /*while(1){ - printf("\r\n"); - - hum_temp->get_temperature(&value1); - hum_temp->get_humidity(&value2); - printf("HTS221: [temp] %4.2f C, [hum] %4.2f%%\r\n", value1, value2); - - press_temp->get_temperature(&value1); - press_temp->get_pressure(&value2); - printf("LPS22HH: [temp] %4.2f C, [press] %6.2f mbar\r\n", value1, value2); - - temp->get_temperature(&value1); - printf("STTS751: [temp] %4.2f C\r\n", value1); - - printf("---\r\n"); - - magnetometer->get_m_axes(axes); - printf("LIS2MDL [mag/mgauss]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - - accelerometer->get_x_axes(axes); - printf("LIS2DW12 [acc/mg]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - - acc_gyro->get_x_axes(axes); - printf("LSM6DSO [acc/mg]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - - acc_gyro->get_g_axes(axes); - printf("LSM6DSO [gyro/mdps]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - }*/ } @@ -102,62 +63,163 @@ } -void CalculateData::enable(){ - //Starten des periodischen Task - ticker.attach(callback(this, &CalculateData::run), PERIOD); + - -} - -void CalculateData::disable(){ - ticker.detach(); // Stoppt den periodischen Task +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; + acc_old[i]=0; + gyro[i]=0; + gyro_old[i]=0; + speed[i]=0; + speed_old[i]=0; + angle[i]=0; + pos[i]=0; + } + + t = 0; + t_old = 0; + + //Timer und Ticker starten + timer.start(); + //ticker.attach(callback(this, &CalculateData::run), PERIOD); } -void CalculateData::getData(){ - printf("\r\n"); - - hum_temp->get_temperature(&value1); - hum_temp->get_humidity(&value2); - printf("HTS221: [temp] %4.2f C, [hum] %4.2f%%\r\n", value1, value2); - - press_temp->get_temperature(&value1); - press_temp->get_pressure(&value2); - printf("LPS22HH: [temp] %4.2f C, [press] %6.2f mbar\r\n", value1, value2); - - temp->get_temperature(&value1); - printf("STTS751: [temp] %4.2f C\r\n", value1); - - printf("---\r\n"); - - magnetometer->get_m_axes(axes); - printf("LIS2MDL [mag/mgauss]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - - accelerometer->get_x_axes(axes); - printf("LIS2DW12 [acc/mg]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - - acc_gyro->get_x_axes(axes); - printf("LSM6DSO [acc/mg]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - - acc_gyro->get_g_axes(axes); - printf("LSM6DSO [gyro/mdps]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]); - -} - - -//Periodischer Task -void CalculateData::run() { - - getData(); - +//Timer und Ticker beenden +void CalculateData::disable(){ + ticker.detach(); + timer.stop(); } +//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]; + } + t_old = t; + + //Sensoren und Timer auslesen + accelerometer->get_x_axes(acc); + acc_gyro->get_g_axes(gyro); + t=timer.read_ms(); + + filter(acc, gyro); //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 + integrate(speed, speed_old, pos, t, t_old); //Integral -> Position +} + + +//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; + + for(int i = 0;i < 3;i++){ + y[i] = y[i] + ((x[i] + x_old[i])/2)*dt; + } + +} + +//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}; + + 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]; + + 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]); + + acc[j] = (int)sum_acc[j]; + gyro[j] = (int)sum_gyro[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]; + } +} +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]; + } +} \ No newline at end of file