Cyrill Zoller / Mbed 2 deprecated calculate

calculate

Dependencies:   mbed X_NUCLEO_IKS01A3 Mahony_Algorithm

CalculateData.cpp@3:795998b31c32, 2020-04-16 (annotated)

Committer:
zollecy1
Date:
Thu Apr 16 19:34:49 2020 +0000
Revision:
3:795998b31c32
Parent:
2:4cccdc792719
Child:
4:7d13076ecece
.

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 3:795998b31c32 30 #include <Thread.h>
zollecy1 3:795998b31c32 31 #include "Liste.h"
zollecy1 0:313fbc3a198a 32
zollecy1 0:313fbc3a198a 33 using namespace std;
zollecy1 0:313fbc3a198a 34
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 2:4cccdc792719 38 const int FILTERSIZE = 28;
zollecy1 1:48e219526d0f 39 static int acc_filter[3][FILTERSIZE];
zollecy1 1:48e219526d0f 40 static int gyro_filter[3][FILTERSIZE];
zollecy1 2:4cccdc792719 41 static int speed_filter[3][FILTERSIZE];
zollecy1 2:4cccdc792719 42 const double filter_koef[FILTERSIZE] = {0.04002232346037, -0.1345598020407, -0.05881026752833, -0.03718019471321,
zollecy1 2:4cccdc792719 43 -0.02581625686635, -0.01315040166028, 0.003361386671892, 0.0234601656009,
zollecy1 2:4cccdc792719 44 0.04581625763125, 0.06897560043795, 0.0905455051097, 0.1088826077838,
zollecy1 2:4cccdc792719 45 0.1221264326899, 0.1290832392388, 0.1290832392388, 0.1221264326899,
zollecy1 2:4cccdc792719 46 0.1088826077838, 0.0905455051097, 0.06897560043795, 0.04581625763125,
zollecy1 2:4cccdc792719 47 0.0234601656009, 0.003361386671892, -0.01315040166028, -0.02581625686635,
zollecy1 2:4cccdc792719 48 -0.03718019471321, -0.05881026752833, -0.1345598020407, 0.04002232346037
zollecy1 2:4cccdc792719 49 };
zollecy1 2:4cccdc792719 50
zollecy1 0:313fbc3a198a 51
zollecy1 0:313fbc3a198a 52
zollecy1 0:313fbc3a198a 53
zollecy1 0:313fbc3a198a 54 CalculateData::CalculateData(PinName p0, PinName p1, PinName p2, PinName p3,
zollecy1 1:48e219526d0f 55 PinName p4, PinName p5, PinName p6){
zollecy1 0:313fbc3a198a 56 /* Instantiate the expansion board */
zollecy1 0:313fbc3a198a 57 mems_expansion_board = XNucleoIKS01A3::instance(p0, p1, p2, p3, p4, p5, p6);
zollecy1 0:313fbc3a198a 58
zollecy1 0:313fbc3a198a 59 /* Retrieve the composing elements of the expansion board */
zollecy1 0:313fbc3a198a 60 acc_gyro = mems_expansion_board->acc_gyro;
zollecy1 0:313fbc3a198a 61 accelerometer = mems_expansion_board->accelerometer;
zollecy1 1:48e219526d0f 62
zollecy1 0:313fbc3a198a 63 /* Enable all sensors */
zollecy1 0:313fbc3a198a 64 accelerometer->enable_x();
zollecy1 1:48e219526d0f 65 //acc_gyro->enable_x();
zollecy1 0:313fbc3a198a 66 acc_gyro->enable_g();
zollecy1 0:313fbc3a198a 67 }
zollecy1 0:313fbc3a198a 68
zollecy1 0:313fbc3a198a 69
zollecy1 0:313fbc3a198a 70 CalculateData::~CalculateData() {
zollecy1 0:313fbc3a198a 71
zollecy1 0:313fbc3a198a 72 }
zollecy1 0:313fbc3a198a 73
zollecy1 0:313fbc3a198a 74
zollecy1 1:48e219526d0f 75
zollecy1 0:313fbc3a198a 76
zollecy1 1:48e219526d0f 77 void CalculateData::enable(){
zollecy1 1:48e219526d0f 78 //Anfangsbedingungen auf NULL
zollecy1 1:48e219526d0f 79 for (int i = 0; i<3; i++){
zollecy1 1:48e219526d0f 80 acc[i]=0;
zollecy1 1:48e219526d0f 81 acc_old[i]=0;
zollecy1 1:48e219526d0f 82 gyro[i]=0;
zollecy1 1:48e219526d0f 83 gyro_old[i]=0;
zollecy1 1:48e219526d0f 84 speed[i]=0;
zollecy1 1:48e219526d0f 85 speed_old[i]=0;
zollecy1 1:48e219526d0f 86 angle[i]=0;
zollecy1 3:795998b31c32 87 pos[i]=0;
zollecy1 1:48e219526d0f 88 }
zollecy1 1:48e219526d0f 89
zollecy1 1:48e219526d0f 90 t_old = 0;
zollecy1 3:795998b31c32 91 counter=0;
zollecy1 1:48e219526d0f 92
zollecy1 3:795998b31c32 93 //Timer und Thread starten
zollecy1 1:48e219526d0f 94 timer.start();
zollecy1 3:795998b31c32 95 periodic_task=true;
zollecy1 3:795998b31c32 96 thread.start(callback(this, &CalculateData::run));
zollecy1 3:795998b31c32 97 thread.set_priority(osPriorityHigh);
zollecy1 0:313fbc3a198a 98 }
zollecy1 0:313fbc3a198a 99
zollecy1 3:795998b31c32 100 //Timer und Thread beenden
zollecy1 1:48e219526d0f 101 void CalculateData::disable(){
zollecy1 3:795998b31c32 102 periodic_task=false;
zollecy1 3:795998b31c32 103 timer.stop();
zollecy1 3:795998b31c32 104 thread.join();
zollecy1 0:313fbc3a198a 105 }
zollecy1 0:313fbc3a198a 106
zollecy1 0:313fbc3a198a 107
zollecy1 0:313fbc3a198a 108
zollecy1 0:313fbc3a198a 109
zollecy1 0:313fbc3a198a 110
zollecy1 1:48e219526d0f 111 //Periodischer Task
zollecy1 1:48e219526d0f 112 void CalculateData::run() {
zollecy1 3:795998b31c32 113 while(periodic_task){
zollecy1 3:795998b31c32 114
zollecy1 3:795998b31c32 115 counter +=1;
zollecy1 3:795998b31c32 116
zollecy1 3:795998b31c32 117 //Sensoren und Timer auslesen
zollecy1 3:795998b31c32 118 accelerometer->get_x_axes(buf_acc);
zollecy1 3:795998b31c32 119 acc_gyro->get_g_axes(buf_gyro);
zollecy1 3:795998b31c32 120 t[counter-1]=(double)timer.read_ms()/1000.0f;
zollecy1 3:795998b31c32 121
zollecy1 3:795998b31c32 122 for(int i=0;i<3;i++){
zollecy1 3:795998b31c32 123 array_acc[i][counter-1]=buf_acc[i];
zollecy1 3:795998b31c32 124 array_gyro[i][counter-1]=buf_gyro[i];
zollecy1 3:795998b31c32 125 }
zollecy1 3:795998b31c32 126
zollecy1 3:795998b31c32 127 Thread::wait(10);
zollecy1 1:48e219526d0f 128 }
zollecy1 1:48e219526d0f 129 }
zollecy1 1:48e219526d0f 130
zollecy1 1:48e219526d0f 131
zollecy1 3:795998b31c32 132
zollecy1 3:795998b31c32 133 void CalculateData::getValue(Liste *list){
zollecy1 3:795998b31c32 134
zollecy1 3:795998b31c32 135 //calculate actual Values
zollecy1 3:795998b31c32 136 for(int i=0;i<counter;i++){
zollecy1 3:795998b31c32 137 filterAcc(array_acc, i, acc);
zollecy1 3:795998b31c32 138 filterGyro(array_gyro, i, gyro);
zollecy1 3:795998b31c32 139 integrate(acc, acc_old, speed, t[i], t_old);
zollecy1 3:795998b31c32 140 integrate(gyro, gyro_old, angle, t[i], t_old);
zollecy1 3:795998b31c32 141 filterSpeed(speed, speed);
zollecy1 3:795998b31c32 142 integrate(speed, speed_old, pos, t[i], t_old);
zollecy1 3:795998b31c32 143
zollecy1 3:795998b31c32 144 for (int j=0; j<3;j++){
zollecy1 3:795998b31c32 145 acc_old[j] = acc[j];
zollecy1 3:795998b31c32 146 gyro_old[j] = gyro[j];
zollecy1 3:795998b31c32 147 speed_old[j] = speed[j];
zollecy1 3:795998b31c32 148 }
zollecy1 3:795998b31c32 149 t_old=t[i];
zollecy1 3:795998b31c32 150 }
zollecy1 3:795998b31c32 151 counter = 0;
zollecy1 3:795998b31c32 152
zollecy1 3:795998b31c32 153 //save Data in List
zollecy1 3:795998b31c32 154 list->accX= acc[0];
zollecy1 3:795998b31c32 155 list->accY= acc[1];
zollecy1 3:795998b31c32 156 list->accZ= acc[2];
zollecy1 3:795998b31c32 157 list->speedX= speed[0];
zollecy1 3:795998b31c32 158 list->speedY= speed[1];
zollecy1 3:795998b31c32 159 list->speedZ= speed[2];
zollecy1 3:795998b31c32 160 list->posX= pos[0];
zollecy1 3:795998b31c32 161 list->posY= pos[1];
zollecy1 3:795998b31c32 162 list->posZ= pos[2];
zollecy1 3:795998b31c32 163 list->gyroX= gyro[0];
zollecy1 3:795998b31c32 164 list->gyroY= gyro[1];
zollecy1 3:795998b31c32 165 list->gyroZ= gyro[2];
zollecy1 3:795998b31c32 166 list->angleX= angle[0];
zollecy1 3:795998b31c32 167 list->angleY= angle[1];
zollecy1 3:795998b31c32 168 list->angleZ= angle[2];
zollecy1 3:795998b31c32 169 list->time= t_old;
zollecy1 3:795998b31c32 170 }
zollecy1 3:795998b31c32 171
zollecy1 3:795998b31c32 172
zollecy1 3:795998b31c32 173
zollecy1 3:795998b31c32 174
zollecy1 3:795998b31c32 175
zollecy1 3:795998b31c32 176
zollecy1 3:795998b31c32 177
zollecy1 3:795998b31c32 178
zollecy1 3:795998b31c32 179
zollecy1 1:48e219526d0f 180 //Daten Integrieren nach expl. Euler
zollecy1 3:795998b31c32 181 void CalculateData::integrate(double *x, double *x_old, double *y, double t, double t_old){
zollecy1 3:795998b31c32 182 double dt = t-t_old;
zollecy1 1:48e219526d0f 183
zollecy1 1:48e219526d0f 184 for(int i = 0;i < 3;i++){
zollecy1 3:795998b31c32 185 y[i] = y[i] + ((x[i]+x_old[i])/2.0f)*dt;
zollecy1 1:48e219526d0f 186 }
zollecy1 1:48e219526d0f 187
zollecy1 1:48e219526d0f 188 }
zollecy1 1:48e219526d0f 189
zollecy1 2:4cccdc792719 190
zollecy1 1:48e219526d0f 191
zollecy1 1:48e219526d0f 192
zollecy1 3:795998b31c32 193 //FIR-Filter (Accelerometer)
zollecy1 3:795998b31c32 194 void CalculateData::filterAcc(int array[3][30],int index, double *target){
zollecy1 2:4cccdc792719 195
zollecy1 2:4cccdc792719 196 double sum_array[] = {0, 0, 0};
zollecy1 1:48e219526d0f 197
zollecy1 3:795998b31c32 198 for(int j=0;j<3;j++){
zollecy1 3:795998b31c32 199 for(int i=0;i<FILTERSIZE-2;i++){
zollecy1 3:795998b31c32 200 acc_filter[j][i] = acc_filter[j][i+1];
zollecy1 3:795998b31c32 201 sum_array[j] += (filter_koef[i] * (double)acc_filter[j][i]);
zollecy1 3:795998b31c32 202 }
zollecy1 3:795998b31c32 203 acc_filter[j][FILTERSIZE-1] = array[j][index];
zollecy1 3:795998b31c32 204 sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)acc_filter[j][FILTERSIZE-1]);
zollecy1 3:795998b31c32 205 target[j] = sum_array[j];
zollecy1 3:795998b31c32 206 }
zollecy1 3:795998b31c32 207
zollecy1 3:795998b31c32 208 }
zollecy1 3:795998b31c32 209
zollecy1 3:795998b31c32 210 //FIR-Filter (Gyro)
zollecy1 3:795998b31c32 211 void CalculateData::filterGyro(int array[3][30],int index, double *target){
zollecy1 3:795998b31c32 212 double sum_array[] = {0, 0, 0};
zollecy1 3:795998b31c32 213
zollecy1 3:795998b31c32 214 for(int j=0;j<3;j++){
zollecy1 3:795998b31c32 215 for(int i=0;i<FILTERSIZE-2;i++){
zollecy1 3:795998b31c32 216 gyro_filter[j][i] = gyro_filter[j][i+1];
zollecy1 3:795998b31c32 217 sum_array[j] += (filter_koef[i] * (double)gyro_filter[j][i]);
zollecy1 3:795998b31c32 218 }
zollecy1 3:795998b31c32 219 gyro_filter[j][FILTERSIZE-1] = array[j][index];
zollecy1 3:795998b31c32 220 sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)gyro_filter[j][FILTERSIZE-1]);
zollecy1 3:795998b31c32 221 target[j] = sum_array[j];
zollecy1 2:4cccdc792719 222 }
zollecy1 1:48e219526d0f 223 }
zollecy1 0:313fbc3a198a 224
zollecy1 3:795998b31c32 225 //FIR-Filter (Speed)
zollecy1 3:795998b31c32 226 void CalculateData::filterSpeed(double *array, double *target){
zollecy1 3:795998b31c32 227 double sum_array[] = {0, 0, 0};
zollecy1 3:795998b31c32 228
zollecy1 3:795998b31c32 229 for(int j=0;j<3;j++){
zollecy1 3:795998b31c32 230 for(int i=0;i<FILTERSIZE-2;i++){
zollecy1 3:795998b31c32 231 speed_filter[j][i] = speed_filter[j][i+1];
zollecy1 3:795998b31c32 232 sum_array[j] += (filter_koef[i] * (double)speed_filter[j][i]);
zollecy1 3:795998b31c32 233 }
zollecy1 3:795998b31c32 234 speed_filter[j][FILTERSIZE-1] = array[j];
zollecy1 3:795998b31c32 235 sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)speed_filter[j][FILTERSIZE-1]);
zollecy1 3:795998b31c32 236 target[j] = sum_array[j];
zollecy1 3:795998b31c32 237 }
zollecy1 1:48e219526d0f 238 }
zollecy1 0:313fbc3a198a 239
zollecy1 0:313fbc3a198a 240
zollecy1 2:4cccdc792719 241
zollecy1 2:4cccdc792719 242 /*
zollecy1 2:4cccdc792719 243 //Beschleunigung in globale Koordinaten Transformieren
zollecy1 2:4cccdc792719 244 void CalculateData::transform(int *acc, int *angle){
zollecy1 2:4cccdc792719 245 double acc_new[3]; //Neue Werte
zollecy1 2:4cccdc792719 246 double a = angle[0]*PI/(1000.0*180.0); //Winkel alpha in rad
zollecy1 2:4cccdc792719 247 double b = angle[1]*PI/(1000.0*180.0); //Winkel beta in rad
zollecy1 2:4cccdc792719 248 double c = angle[2]*PI/(1000.0*180.0); //Winkel gamma in rad
zollecy1 2:4cccdc792719 249
zollecy1 2:4cccdc792719 250 //Transformation
zollecy1 2:4cccdc792719 251 acc_new[0] = (double)acc[0]*cos(a)*cos(c) - (double)acc[1]*cos(b)*sin(c) + (double)acc[2]*sin(b);
zollecy1 2:4cccdc792719 252 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 2:4cccdc792719 253 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 2:4cccdc792719 254
zollecy1 2:4cccdc792719 255 for(int i=0;i<3;i++){
zollecy1 2:4cccdc792719 256 acc[i] = (int)acc_new[i];
zollecy1 2:4cccdc792719 257 }
zollecy1 2:4cccdc792719 258 }
zollecy1 2:4cccdc792719 259 */