Cyrill Zoller
calculate
Dependencies: mbed X_NUCLEO_IKS01A3 Mahony_Algorithm
Diff: CalculateData.cpp
- Revision:
- 1:48e219526d0f
- Parent:
- 0:313fbc3a198a
- Child:
- 2:4cccdc792719
diff -r 313fbc3a198a -r 48e219526d0f CalculateData.cpp
--- 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