Cyrill Zoller / Mbed 2 deprecated calculate

calculate

Dependencies:   mbed X_NUCLEO_IKS01A3 Mahony_Algorithm

CalculateData.cpp@1:48e219526d0f, 2020-04-13 (annotated)

Committer:
zollecy1
Date:
Mon Apr 13 09:37:17 2020 +0000
Revision:
1:48e219526d0f
Parent:
0:313fbc3a198a
Child:
2:4cccdc792719
.;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zollecy1 0:313fbc3a198a 1 /**
zollecy1 0:313fbc3a198a 2 |**********************************************************************;
zollecy1 0:313fbc3a198a 3 * Project : Projektarbeit Systemtechnik PES4
zollecy1 0:313fbc3a198a 4 *
zollecy1 0:313fbc3a198a 5 * Program name : Beispiel
zollecy1 0:313fbc3a198a 6 *
zollecy1 0:313fbc3a198a 7 * Author : PES4 Team1
zollecy1 0:313fbc3a198a 8 *
zollecy1 0:313fbc3a198a 9 * Team : **Team 1**
zollecy1 0:313fbc3a198a 10 * Fabio Bernard
zollecy1 0:313fbc3a198a 11 * Lukas Egli
zollecy1 0:313fbc3a198a 12 * Matthias Ott
zollecy1 0:313fbc3a198a 13 * Pascal Novacki
zollecy1 0:313fbc3a198a 14 * Robin Wanner
zollecy1 0:313fbc3a198a 15 * Vincent Vescoli
zollecy1 0:313fbc3a198a 16 * Cyrill Zoller
zollecy1 0:313fbc3a198a 17 *
zollecy1 0:313fbc3a198a 18 * Date created : 20.02.2020
zollecy1 0:313fbc3a198a 19 *
zollecy1 0:313fbc3a198a 20 * Purpose : Beispiel
zollecy1 0:313fbc3a198a 21 *
zollecy1 0:313fbc3a198a 22 |**********************************************************************;
zollecy1 0:313fbc3a198a 23 **/
zollecy1 0:313fbc3a198a 24
zollecy1 1:48e219526d0f 25 #define PI (3.14159265)
zollecy1 0:313fbc3a198a 26 #include "CalculateData.h"
zollecy1 0:313fbc3a198a 27 #include "mbed.h"
zollecy1 0:313fbc3a198a 28 #include "XNucleoIKS01A3.h"
zollecy1 1:48e219526d0f 29 #include <math.h>
zollecy1 0:313fbc3a198a 30
zollecy1 0:313fbc3a198a 31 using namespace std;
zollecy1 0:313fbc3a198a 32
zollecy1 1:48e219526d0f 33 const float CalculateData::PERIOD = 1.0f; //Periode von 10 ms
zollecy1 1:48e219526d0f 34 const int FILTERSIZE = 10;
zollecy1 1:48e219526d0f 35 static XNucleoIKS01A3 *mems_expansion_board; //Sensor-Board
zollecy1 1:48e219526d0f 36 static LSM6DSOSensor *acc_gyro; //Gyro-Sensor
zollecy1 1:48e219526d0f 37 static LIS2DW12Sensor *accelerometer; //Acceleroation-Sensor
zollecy1 1:48e219526d0f 38 static int acc_filter[3][FILTERSIZE];
zollecy1 1:48e219526d0f 39 static int gyro_filter[3][FILTERSIZE];
zollecy1 1:48e219526d0f 40 const double filter_koef[] = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1};
zollecy1 0:313fbc3a198a 41
zollecy1 0:313fbc3a198a 42
zollecy1 0:313fbc3a198a 43
zollecy1 0:313fbc3a198a 44
zollecy1 0:313fbc3a198a 45 CalculateData::CalculateData(PinName p0, PinName p1, PinName p2, PinName p3,
zollecy1 1:48e219526d0f 46 PinName p4, PinName p5, PinName p6){
zollecy1 0:313fbc3a198a 47 /* Instantiate the expansion board */
zollecy1 0:313fbc3a198a 48 mems_expansion_board = XNucleoIKS01A3::instance(p0, p1, p2, p3, p4, p5, p6);
zollecy1 0:313fbc3a198a 49
zollecy1 0:313fbc3a198a 50 /* Retrieve the composing elements of the expansion board */
zollecy1 0:313fbc3a198a 51 acc_gyro = mems_expansion_board->acc_gyro;
zollecy1 0:313fbc3a198a 52 accelerometer = mems_expansion_board->accelerometer;
zollecy1 1:48e219526d0f 53
zollecy1 0:313fbc3a198a 54 /* Enable all sensors */
zollecy1 0:313fbc3a198a 55 accelerometer->enable_x();
zollecy1 1:48e219526d0f 56 //acc_gyro->enable_x();
zollecy1 0:313fbc3a198a 57 acc_gyro->enable_g();
zollecy1 0:313fbc3a198a 58 }
zollecy1 0:313fbc3a198a 59
zollecy1 0:313fbc3a198a 60
zollecy1 0:313fbc3a198a 61 CalculateData::~CalculateData() {
zollecy1 0:313fbc3a198a 62
zollecy1 0:313fbc3a198a 63 }
zollecy1 0:313fbc3a198a 64
zollecy1 0:313fbc3a198a 65
zollecy1 1:48e219526d0f 66
zollecy1 0:313fbc3a198a 67
zollecy1 1:48e219526d0f 68 void CalculateData::enable(){
zollecy1 1:48e219526d0f 69 //Mittelwert der Fehler der Sensoren mit g berechnen
zollecy1 1:48e219526d0f 70 acc_err = {0, 0, 0};
zollecy1 1:48e219526d0f 71 gyro_err = {0, 0, 0};
zollecy1 1:48e219526d0f 72 for(int i=0;i<10;i++){
zollecy1 1:48e219526d0f 73 accelerometer->get_x_axes(acc);
zollecy1 1:48e219526d0f 74 acc_gyro->get_g_axes(gyro);
zollecy1 1:48e219526d0f 75 for(int k=0;k<3;k++){
zollecy1 1:48e219526d0f 76 acc_err[i] += acc[i];
zollecy1 1:48e219526d0f 77 gyro_err[i] += gyro[i];
zollecy1 1:48e219526d0f 78 }
zollecy1 1:48e219526d0f 79 }
zollecy1 1:48e219526d0f 80
zollecy1 1:48e219526d0f 81 for(int i=0; i<3; i++){
zollecy1 1:48e219526d0f 82 acc_err[i] /= 10;
zollecy1 1:48e219526d0f 83 gyro_err[i] /= 10;
zollecy1 1:48e219526d0f 84 }
zollecy1 1:48e219526d0f 85
zollecy1 1:48e219526d0f 86 //Anfangsbedingungen auf NULL
zollecy1 1:48e219526d0f 87 for (int i = 0; i<3; i++){
zollecy1 1:48e219526d0f 88 acc[i]=0;
zollecy1 1:48e219526d0f 89 acc_old[i]=0;
zollecy1 1:48e219526d0f 90 gyro[i]=0;
zollecy1 1:48e219526d0f 91 gyro_old[i]=0;
zollecy1 1:48e219526d0f 92 speed[i]=0;
zollecy1 1:48e219526d0f 93 speed_old[i]=0;
zollecy1 1:48e219526d0f 94 angle[i]=0;
zollecy1 1:48e219526d0f 95 pos[i]=0;
zollecy1 1:48e219526d0f 96 }
zollecy1 1:48e219526d0f 97
zollecy1 1:48e219526d0f 98 t = 0;
zollecy1 1:48e219526d0f 99 t_old = 0;
zollecy1 1:48e219526d0f 100
zollecy1 1:48e219526d0f 101 //Timer und Ticker starten
zollecy1 1:48e219526d0f 102 timer.start();
zollecy1 1:48e219526d0f 103 //ticker.attach(callback(this, &CalculateData::run), PERIOD);
zollecy1 0:313fbc3a198a 104 }
zollecy1 0:313fbc3a198a 105
zollecy1 1:48e219526d0f 106 //Timer und Ticker beenden
zollecy1 1:48e219526d0f 107 void CalculateData::disable(){
zollecy1 1:48e219526d0f 108 ticker.detach();
zollecy1 1:48e219526d0f 109 timer.stop();
zollecy1 0:313fbc3a198a 110 }
zollecy1 0:313fbc3a198a 111
zollecy1 0:313fbc3a198a 112
zollecy1 0:313fbc3a198a 113
zollecy1 0:313fbc3a198a 114
zollecy1 0:313fbc3a198a 115
zollecy1 1:48e219526d0f 116 //Periodischer Task
zollecy1 1:48e219526d0f 117 void CalculateData::run() {
zollecy1 1:48e219526d0f 118 //Letzte Werte speichern
zollecy1 1:48e219526d0f 119 for (int i=0; i<3;i++){
zollecy1 1:48e219526d0f 120 acc_old[i] = acc[i];
zollecy1 1:48e219526d0f 121 gyro_old[i] = gyro[i];
zollecy1 1:48e219526d0f 122 speed_old[i] = speed[i];
zollecy1 1:48e219526d0f 123 }
zollecy1 1:48e219526d0f 124 t_old = t;
zollecy1 1:48e219526d0f 125
zollecy1 1:48e219526d0f 126 //Sensoren und Timer auslesen
zollecy1 1:48e219526d0f 127 accelerometer->get_x_axes(acc);
zollecy1 1:48e219526d0f 128 acc_gyro->get_g_axes(gyro);
zollecy1 1:48e219526d0f 129 t=timer.read_ms();
zollecy1 1:48e219526d0f 130
zollecy1 1:48e219526d0f 131 filter(acc, gyro); //Daten Filtern
zollecy1 1:48e219526d0f 132 integrate(gyro, gyro_old, angle, t, t_old); //Integral -> Winkel
zollecy1 1:48e219526d0f 133 transform(acc, angle); //Beschleunigung in Globale Koordinaten transformieren
zollecy1 1:48e219526d0f 134 integrate(acc, acc_old, speed, t, t_old); //Integral -> Geschwindigkeit
zollecy1 1:48e219526d0f 135 integrate(speed, speed_old, pos, t, t_old); //Integral -> Position
zollecy1 1:48e219526d0f 136 }
zollecy1 1:48e219526d0f 137
zollecy1 1:48e219526d0f 138
zollecy1 1:48e219526d0f 139 //Daten Integrieren nach expl. Euler
zollecy1 1:48e219526d0f 140 void CalculateData::integrate(int *x, int *x_old, int *y, int t, int t_old){
zollecy1 1:48e219526d0f 141 int dt = t-t_old;
zollecy1 1:48e219526d0f 142
zollecy1 1:48e219526d0f 143 for(int i = 0;i < 3;i++){
zollecy1 1:48e219526d0f 144 y[i] = y[i] + ((x[i] + x_old[i])/2)*dt;
zollecy1 1:48e219526d0f 145 }
zollecy1 1:48e219526d0f 146
zollecy1 1:48e219526d0f 147 }
zollecy1 1:48e219526d0f 148
zollecy1 1:48e219526d0f 149 //Beschleunigung in globale Koordinaten Transformieren
zollecy1 1:48e219526d0f 150 void CalculateData::transform(int *acc, int *angle){
zollecy1 1:48e219526d0f 151 double acc_new[3]; //Neue Werte
zollecy1 1:48e219526d0f 152 double a = angle[0]*PI/(1000.0*180.0); //Winkel alpha in rad
zollecy1 1:48e219526d0f 153 double b = angle[1]*PI/(1000.0*180.0); //Winkel beta in rad
zollecy1 1:48e219526d0f 154 double c = angle[2]*PI/(1000.0*180.0); //Winkel gamma in rad
zollecy1 1:48e219526d0f 155
zollecy1 1:48e219526d0f 156 //Transformation
zollecy1 1:48e219526d0f 157 acc_new[0] = (double)acc[0]*cos(a)*cos(c) - (double)acc[1]*cos(b)*sin(c) + (double)acc[2]*sin(b);
zollecy1 1:48e219526d0f 158 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);
zollecy1 1:48e219526d0f 159 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);
zollecy1 1:48e219526d0f 160
zollecy1 1:48e219526d0f 161 for(int i=0;i<3;i++){
zollecy1 1:48e219526d0f 162 acc[i] = (int)acc_new[i];
zollecy1 1:48e219526d0f 163 }
zollecy1 1:48e219526d0f 164 }
zollecy1 1:48e219526d0f 165
zollecy1 1:48e219526d0f 166
zollecy1 1:48e219526d0f 167 //Filtern der Daten mittels FIR-Filter
zollecy1 1:48e219526d0f 168 void CalculateData::filter(int *acc, int *gyro){
zollecy1 1:48e219526d0f 169 double sum_acc[] = {0, 0, 0};
zollecy1 1:48e219526d0f 170 double sum_gyro[] = {0, 0, 0};
zollecy1 1:48e219526d0f 171
zollecy1 1:48e219526d0f 172 for(int j=0;j<3;j++){
zollecy1 1:48e219526d0f 173 for(int i=0;i<FILTERSIZE-2;i++){
zollecy1 1:48e219526d0f 174 acc_filter[j][i] = acc_filter[j][i+1];
zollecy1 1:48e219526d0f 175 gyro_filter[j][i] = gyro_filter[j][i+1];
zollecy1 1:48e219526d0f 176 sum_acc[j] += (filter_koef[i] * (double)acc_filter[j][i]);
zollecy1 1:48e219526d0f 177 sum_gyro[j] += (filter_koef[i] * (double)gyro_filter[j][i]);
zollecy1 1:48e219526d0f 178 }
zollecy1 1:48e219526d0f 179 acc_filter[j][FILTERSIZE-1] = acc[j];
zollecy1 1:48e219526d0f 180 gyro_filter[j][FILTERSIZE-1] = gyro[j];
zollecy1 1:48e219526d0f 181
zollecy1 1:48e219526d0f 182 sum_acc[j] += (filter_koef[FILTERSIZE-1] * (double)acc_filter[j][FILTERSIZE-1]);
zollecy1 1:48e219526d0f 183 sum_gyro[j] += (filter_koef[FILTERSIZE-1] * (double)gyro_filter[j][FILTERSIZE-1]);
zollecy1 1:48e219526d0f 184
zollecy1 1:48e219526d0f 185 acc[j] = (int)sum_acc[j];
zollecy1 1:48e219526d0f 186 gyro[j] = (int)sum_gyro[j];
zollecy1 1:48e219526d0f 187 }
zollecy1 1:48e219526d0f 188 }
zollecy1 0:313fbc3a198a 189
zollecy1 0:313fbc3a198a 190
zollecy1 0:313fbc3a198a 191
zollecy1 0:313fbc3a198a 192
zollecy1 1:48e219526d0f 193 void CalculateData::getAccelerometer(int *array){
zollecy1 1:48e219526d0f 194 for(int i=0;i<3;i++){
zollecy1 1:48e219526d0f 195 array[i] = acc[i];
zollecy1 1:48e219526d0f 196 }
zollecy1 1:48e219526d0f 197 }
zollecy1 1:48e219526d0f 198
zollecy1 1:48e219526d0f 199
zollecy1 1:48e219526d0f 200 void CalculateData::getSpeed(int *array){
zollecy1 1:48e219526d0f 201 for(int i=0;i<3;i++){
zollecy1 1:48e219526d0f 202 array[i] = speed[i];
zollecy1 1:48e219526d0f 203 }
zollecy1 1:48e219526d0f 204 }
zollecy1 0:313fbc3a198a 205
zollecy1 0:313fbc3a198a 206
zollecy1 1:48e219526d0f 207 void CalculateData::getPosition(int *array){
zollecy1 1:48e219526d0f 208 for(int i=0;i<3;i++){
zollecy1 1:48e219526d0f 209 array[i] = pos[i];
zollecy1 1:48e219526d0f 210 }
zollecy1 1:48e219526d0f 211 }
zollecy1 1:48e219526d0f 212
zollecy1 1:48e219526d0f 213
zollecy1 1:48e219526d0f 214 void CalculateData::getGyro(int *array){
zollecy1 1:48e219526d0f 215 for(int i=0;i<3;i++){
zollecy1 1:48e219526d0f 216 array[i] = gyro[i];
zollecy1 1:48e219526d0f 217 }
zollecy1 1:48e219526d0f 218 }
zollecy1 1:48e219526d0f 219
zollecy1 1:48e219526d0f 220
zollecy1 1:48e219526d0f 221 void CalculateData::getAngle(int *array){
zollecy1 1:48e219526d0f 222 for(int i=0;i<3;i++){
zollecy1 1:48e219526d0f 223 array[i] = angle[i];
zollecy1 1:48e219526d0f 224 }
zollecy1 1:48e219526d0f 225 }