Cyrill Zoller / Mbed 2 deprecated calculate

calculate

Dependencies:   mbed X_NUCLEO_IKS01A3 Mahony_Algorithm

CalculateData.cpp@6:4641f2b2684b, 2020-05-01 (annotated)

Committer:
zollecy1
Date:
Fri May 01 08:44:21 2020 +0000
Revision:
6:4641f2b2684b
Parent:
5:62994bb9fff9 
..

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 4:7d13076ecece 32 #include "MahonyAHRS.h"
zollecy1 0:313fbc3a198a 33
zollecy1 0:313fbc3a198a 34 using namespace std;
zollecy1 0:313fbc3a198a 35
zollecy1 1:48e219526d0f 36 static XNucleoIKS01A3 *mems_expansion_board; //Sensor-Board
zollecy1 1:48e219526d0f 37 static LSM6DSOSensor *acc_gyro; //Gyro-Sensor
zollecy1 1:48e219526d0f 38 static LIS2DW12Sensor *accelerometer; //Acceleroation-Sensor
zollecy1 4:7d13076ecece 39 static LIS2MDLSensor *magnetometer; //Magnetometer
zollecy1 5:62994bb9fff9 40 const float accOffset[3] = {-0.007734011220907f, -0.007085474965694f, -9.910594323583762f};
zollecy1 2:4cccdc792719 41 const int FILTERSIZE = 28;
zollecy1 5:62994bb9fff9 42 static float acc_filter[3][FILTERSIZE];
zollecy1 5:62994bb9fff9 43 static float gyro_filter[3][FILTERSIZE];
zollecy1 5:62994bb9fff9 44 static float speed_filter[3][FILTERSIZE];
zollecy1 5:62994bb9fff9 45 float magtransformationmatrix[3][3]={{1.624f, 0.049f, 0.011f}, {-0.004f, 1.668f, 0.001f}, {0.074f, 0.001f, 1.635f}};
zollecy1 5:62994bb9fff9 46 float bias[3]={-466.649f, -273.636f, 58.044f};
zollecy1 5:62994bb9fff9 47 const float filter_koef[FILTERSIZE] = {1.54459639027449f, -5.19311640449656f, -2.26968649197199f, -1.43490906020467f,
zollecy1 5:62994bb9fff9 48 -0.996336387257677f, -0.507518334242666f, 0.129727243968288f, 0.905406882193463f,
zollecy1 5:62994bb9fff9 49 1.76820384311742f, 2.66200095951371f, 3.49445629978250f, 4.20214691216170f,
zollecy1 5:62994bb9fff9 50 4.71327076441993f, 4.98175738274206f, 4.98175738274206f, 4.71327076441993f,
zollecy1 5:62994bb9fff9 51 4.20214691216170f, 3.49445629978250f, 2.66200095951371f, 1.76820384311742f,
zollecy1 5:62994bb9fff9 52 0.905406882193463f, 0.129727243968288f, -0.507518334242666f, -0.996336387257677f,
zollecy1 5:62994bb9fff9 53 -1.43490906020467f, -2.26968649197199f, -5.19311640449656f, 1.54459639027449f};
zollecy1 0:313fbc3a198a 54
zollecy1 0:313fbc3a198a 55
zollecy1 0:313fbc3a198a 56
zollecy1 0:313fbc3a198a 57 CalculateData::CalculateData(PinName p0, PinName p1, PinName p2, PinName p3,
zollecy1 1:48e219526d0f 58 PinName p4, PinName p5, PinName p6){
zollecy1 0:313fbc3a198a 59 /* Instantiate the expansion board */
zollecy1 0:313fbc3a198a 60 mems_expansion_board = XNucleoIKS01A3::instance(p0, p1, p2, p3, p4, p5, p6);
zollecy1 0:313fbc3a198a 61
zollecy1 0:313fbc3a198a 62 /* Retrieve the composing elements of the expansion board */
zollecy1 0:313fbc3a198a 63 acc_gyro = mems_expansion_board->acc_gyro;
zollecy1 0:313fbc3a198a 64 accelerometer = mems_expansion_board->accelerometer;
zollecy1 4:7d13076ecece 65 magnetometer = mems_expansion_board->magnetometer;
zollecy1 1:48e219526d0f 66
zollecy1 0:313fbc3a198a 67 /* Enable all sensors */
zollecy1 0:313fbc3a198a 68 accelerometer->enable_x();
zollecy1 4:7d13076ecece 69 acc_gyro->enable_x();
zollecy1 0:313fbc3a198a 70 acc_gyro->enable_g();
zollecy1 4:7d13076ecece 71 magnetometer->enable();
zollecy1 4:7d13076ecece 72
zollecy1 4:7d13076ecece 73 //initialisation Mahony
zollecy1 5:62994bb9fff9 74 mahony = new MahonyAHRS(200.0);
zollecy1 0:313fbc3a198a 75 }
zollecy1 0:313fbc3a198a 76
zollecy1 0:313fbc3a198a 77
zollecy1 0:313fbc3a198a 78 CalculateData::~CalculateData() {
zollecy1 0:313fbc3a198a 79
zollecy1 0:313fbc3a198a 80 }
zollecy1 0:313fbc3a198a 81
zollecy1 0:313fbc3a198a 82
zollecy1 1:48e219526d0f 83
zollecy1 0:313fbc3a198a 84
zollecy1 1:48e219526d0f 85 void CalculateData::enable(){
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 speed[i]=0;
zollecy1 1:48e219526d0f 91 speed_old[i]=0;
zollecy1 3:795998b31c32 92 pos[i]=0;
zollecy1 1:48e219526d0f 93 }
zollecy1 1:48e219526d0f 94
zollecy1 1:48e219526d0f 95 t_old = 0;
zollecy1 3:795998b31c32 96 counter=0;
zollecy1 1:48e219526d0f 97
zollecy1 3:795998b31c32 98 //Timer und Thread starten
zollecy1 1:48e219526d0f 99 timer.start();
zollecy1 3:795998b31c32 100 periodic_task=true;
zollecy1 3:795998b31c32 101 thread.start(callback(this, &CalculateData::run));
zollecy1 3:795998b31c32 102 thread.set_priority(osPriorityHigh);
zollecy1 0:313fbc3a198a 103 }
zollecy1 0:313fbc3a198a 104
zollecy1 3:795998b31c32 105 //Timer und Thread beenden
zollecy1 1:48e219526d0f 106 void CalculateData::disable(){
zollecy1 3:795998b31c32 107 periodic_task=false;
zollecy1 3:795998b31c32 108 timer.stop();
zollecy1 3:795998b31c32 109 thread.join();
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 4:7d13076ecece 118
zollecy1 4:7d13076ecece 119 int buf_acc[3];
zollecy1 4:7d13076ecece 120 int buf_gyro[3];
zollecy1 4:7d13076ecece 121 int buf_mag[3];
zollecy1 4:7d13076ecece 122
zollecy1 3:795998b31c32 123 while(periodic_task){
zollecy1 3:795998b31c32 124
zollecy1 3:795998b31c32 125 counter +=1;
zollecy1 3:795998b31c32 126
zollecy1 3:795998b31c32 127 //Sensoren und Timer auslesen
zollecy1 4:7d13076ecece 128 //accelerometer->get_x_axes(buf_acc);
zollecy1 4:7d13076ecece 129 acc_gyro->get_x_axes(buf_acc);
zollecy1 3:795998b31c32 130 acc_gyro->get_g_axes(buf_gyro);
zollecy1 4:7d13076ecece 131 magnetometer->get_m_axes(buf_mag);
zollecy1 5:62994bb9fff9 132 t[counter-1]=(float)timer.read_ms();
zollecy1 3:795998b31c32 133
zollecy1 3:795998b31c32 134 for(int i=0;i<3;i++){
zollecy1 3:795998b31c32 135 array_acc[i][counter-1]=buf_acc[i];
zollecy1 3:795998b31c32 136 array_gyro[i][counter-1]=buf_gyro[i];
zollecy1 4:7d13076ecece 137 array_mag[i][counter-1]=buf_mag[i];
zollecy1 3:795998b31c32 138 }
zollecy1 3:795998b31c32 139
zollecy1 4:7d13076ecece 140 Thread::wait(2);
zollecy1 1:48e219526d0f 141 }
zollecy1 1:48e219526d0f 142 }
zollecy1 1:48e219526d0f 143
zollecy1 1:48e219526d0f 144
zollecy1 3:795998b31c32 145
zollecy1 3:795998b31c32 146 void CalculateData::getValue(Liste *list){
zollecy1 4:7d13076ecece 147 //udates the Values
zollecy1 4:7d13076ecece 148 update();
zollecy1 3:795998b31c32 149
zollecy1 3:795998b31c32 150 //save Data in List
zollecy1 3:795998b31c32 151 list->accX= acc[0];
zollecy1 3:795998b31c32 152 list->accY= acc[1];
zollecy1 3:795998b31c32 153 list->accZ= acc[2];
zollecy1 3:795998b31c32 154 list->speedX= speed[0];
zollecy1 3:795998b31c32 155 list->speedY= speed[1];
zollecy1 3:795998b31c32 156 list->speedZ= speed[2];
zollecy1 3:795998b31c32 157 list->posX= pos[0];
zollecy1 3:795998b31c32 158 list->posY= pos[1];
zollecy1 3:795998b31c32 159 list->posZ= pos[2];
zollecy1 3:795998b31c32 160 list->time= t_old;
zollecy1 3:795998b31c32 161 }
zollecy1 3:795998b31c32 162
zollecy1 3:795998b31c32 163
zollecy1 4:7d13076ecece 164 //calculate actual Values
zollecy1 4:7d13076ecece 165 void CalculateData::update(){
zollecy1 4:7d13076ecece 166
zollecy1 4:7d13076ecece 167 for(int i=0;i<counter;i++){
zollecy1 5:62994bb9fff9 168
zollecy1 5:62994bb9fff9 169 float buff[3];
zollecy1 5:62994bb9fff9 170
zollecy1 5:62994bb9fff9 171 calibrationMag(array_mag[0][i],array_mag[1][i],array_mag[2][i], buff);
zollecy1 5:62994bb9fff9 172
zollecy1 5:62994bb9fff9 173 mahony->update(-array_gyro[2][i]*PI/180000.0f, array_gyro[0][i]*PI/180000.0f, array_gyro[1][i]*PI/180000.0f,
zollecy1 5:62994bb9fff9 174 -array_acc[2][i]/1000.0f, array_acc[0][i]/1000.0f, array_acc[1][i]/1000.0f,
zollecy1 5:62994bb9fff9 175 buff[2], buff[0], -buff[1]);
zollecy1 5:62994bb9fff9 176
zollecy1 5:62994bb9fff9 177 mahony->getEuler();
zollecy1 5:62994bb9fff9 178 transform(array_acc, i, acc, mahony->getRoll(), mahony->getPitch(), mahony->getYaw());
zollecy1 5:62994bb9fff9 179
zollecy1 6:4641f2b2684b 180 filterAcc(acc);
zollecy1 5:62994bb9fff9 181
zollecy1 5:62994bb9fff9 182 integrate(acc, acc_old, speed, t[i], t_old); //in mm/s
zollecy1 5:62994bb9fff9 183
zollecy1 5:62994bb9fff9 184 //filterSpeed(speed);
zollecy1 5:62994bb9fff9 185
zollecy1 5:62994bb9fff9 186 integrate(speed, speed_old, pos, t[i], t_old); //in um
zollecy1 4:7d13076ecece 187
zollecy1 4:7d13076ecece 188 for (int j=0; j<3;j++){
zollecy1 4:7d13076ecece 189 acc_old[j] = acc[j];
zollecy1 4:7d13076ecece 190 speed_old[j] = speed[j];
zollecy1 4:7d13076ecece 191 }
zollecy1 5:62994bb9fff9 192 t_old=t[i];
zollecy1 4:7d13076ecece 193 }
zollecy1 4:7d13076ecece 194 counter = 0;
zollecy1 6:4641f2b2684b 195 printf("Roll:\t%i\tPitch:\t%i\tYaw:\t%i\r\n",mahony->getRoll(),mahony->getPitch(),mahony->getYaw());
zollecy1 6:4641f2b2684b 196 //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);
zollecy1 5:62994bb9fff9 197 //printf("%f, %f, %f\r\n",acc[0],acc[1],acc[2]);
zollecy1 4:7d13076ecece 198 }
zollecy1 3:795998b31c32 199
zollecy1 3:795998b31c32 200
zollecy1 3:795998b31c32 201
zollecy1 3:795998b31c32 202
zollecy1 3:795998b31c32 203
zollecy1 3:795998b31c32 204
zollecy1 1:48e219526d0f 205 //Daten Integrieren nach expl. Euler
zollecy1 5:62994bb9fff9 206 void CalculateData::integrate(float *x, float *x_old, float *y, float t, float t_old){
zollecy1 5:62994bb9fff9 207 float dt = t-t_old;
zollecy1 1:48e219526d0f 208
zollecy1 1:48e219526d0f 209 for(int i = 0;i < 3;i++){
zollecy1 3:795998b31c32 210 y[i] = y[i] + ((x[i]+x_old[i])/2.0f)*dt;
zollecy1 1:48e219526d0f 211 }
zollecy1 1:48e219526d0f 212
zollecy1 1:48e219526d0f 213 }
zollecy1 1:48e219526d0f 214
zollecy1 2:4cccdc792719 215
zollecy1 1:48e219526d0f 216
zollecy1 1:48e219526d0f 217
zollecy1 3:795998b31c32 218 //FIR-Filter (Accelerometer)
zollecy1 5:62994bb9fff9 219 void CalculateData::filterAcc(float *array){
zollecy1 2:4cccdc792719 220
zollecy1 2:4cccdc792719 221 double sum_array[] = {0, 0, 0};
zollecy1 1:48e219526d0f 222
zollecy1 3:795998b31c32 223 for(int j=0;j<3;j++){
zollecy1 3:795998b31c32 224 for(int i=0;i<FILTERSIZE-2;i++){
zollecy1 3:795998b31c32 225 acc_filter[j][i] = acc_filter[j][i+1];
zollecy1 5:62994bb9fff9 226 sum_array[j] += (filter_koef[i] * (float)acc_filter[j][i]);
zollecy1 3:795998b31c32 227 }
zollecy1 5:62994bb9fff9 228 acc_filter[j][FILTERSIZE-1] = array[j];
zollecy1 5:62994bb9fff9 229 sum_array[j] += (filter_koef[FILTERSIZE-1] * (float)acc_filter[j][FILTERSIZE-1]);
zollecy1 5:62994bb9fff9 230 array[j] = sum_array[j]/FILTERSIZE;
zollecy1 3:795998b31c32 231 }
zollecy1 3:795998b31c32 232
zollecy1 3:795998b31c32 233 }
zollecy1 3:795998b31c32 234
zollecy1 0:313fbc3a198a 235
zollecy1 3:795998b31c32 236 //FIR-Filter (Speed)
zollecy1 5:62994bb9fff9 237 void CalculateData::filterSpeed(float *array){
zollecy1 3:795998b31c32 238 double sum_array[] = {0, 0, 0};
zollecy1 3:795998b31c32 239
zollecy1 3:795998b31c32 240 for(int j=0;j<3;j++){
zollecy1 3:795998b31c32 241 for(int i=0;i<FILTERSIZE-2;i++){
zollecy1 3:795998b31c32 242 speed_filter[j][i] = speed_filter[j][i+1];
zollecy1 5:62994bb9fff9 243 sum_array[j] += (filter_koef[i] * (float)speed_filter[j][i]);
zollecy1 3:795998b31c32 244 }
zollecy1 3:795998b31c32 245 speed_filter[j][FILTERSIZE-1] = array[j];
zollecy1 5:62994bb9fff9 246 sum_array[j] += (filter_koef[FILTERSIZE-1] * (float)speed_filter[j][FILTERSIZE-1]);
zollecy1 5:62994bb9fff9 247 array[j] = sum_array[j]/FILTERSIZE;
zollecy1 3:795998b31c32 248 }
zollecy1 1:48e219526d0f 249 }
zollecy1 0:313fbc3a198a 250
zollecy1 0:313fbc3a198a 251
zollecy1 5:62994bb9fff9 252 void CalculateData::calibrationMag(int x,int y,int z, float *buff){
zollecy1 5:62994bb9fff9 253 buff[0] = magtransformationmatrix[0][0]*((float)x-bias[0])+
zollecy1 5:62994bb9fff9 254 magtransformationmatrix[0][1]*((float)y-bias[1])+
zollecy1 5:62994bb9fff9 255 magtransformationmatrix[0][2]*((float)z-bias[2]);
zollecy1 5:62994bb9fff9 256
zollecy1 5:62994bb9fff9 257 buff[1] = magtransformationmatrix[1][0]*((float)x-bias[0])+
zollecy1 5:62994bb9fff9 258 magtransformationmatrix[1][1]*((float)y-bias[1])+
zollecy1 5:62994bb9fff9 259 magtransformationmatrix[1][2]*((float)z-bias[2]);
zollecy1 5:62994bb9fff9 260
zollecy1 5:62994bb9fff9 261 buff[2] = magtransformationmatrix[2][0]*((float)x-bias[0])+
zollecy1 5:62994bb9fff9 262 magtransformationmatrix[2][1]*((float)y-bias[1])+
zollecy1 5:62994bb9fff9 263 magtransformationmatrix[2][2]*((float)z-bias[2]);
zollecy1 5:62994bb9fff9 264 }
zollecy1 2:4cccdc792719 265
zollecy1 5:62994bb9fff9 266
zollecy1 2:4cccdc792719 267 //Beschleunigung in globale Koordinaten Transformieren
zollecy1 5:62994bb9fff9 268 void CalculateData::transform(int old_val[3][100], int index, float *new_val, int roll, int pitch, int yaw){
zollecy1 5:62994bb9fff9 269
zollecy1 5:62994bb9fff9 270 //Koeffizienten zum umrechnen
zollecy1 5:62994bb9fff9 271 float sa = sin((float)yaw*PI/180.0f);
zollecy1 5:62994bb9fff9 272 float ca = cos((float)yaw*PI/180.0f);
zollecy1 5:62994bb9fff9 273 float sb = sin((float)(-pitch)*PI/180.0f);
zollecy1 5:62994bb9fff9 274 float cb = cos((float)(-pitch)*PI/180.0f);
zollecy1 5:62994bb9fff9 275 float sc = sin((float)roll*PI/180.0f);
zollecy1 5:62994bb9fff9 276 float cc = cos((float)roll*PI/180.0f);
zollecy1 2:4cccdc792719 277
zollecy1 2:4cccdc792719 278 //Transformation
zollecy1 5:62994bb9fff9 279 /*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);
zollecy1 5:62994bb9fff9 280 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);
zollecy1 5:62994bb9fff9 281 new_val[2] = -(float)old_val[2][index]*(-sb) - (float)old_val[0][index]*(cb*sc) + (float)old_val[1][index]*(cb*cc);
zollecy1 5:62994bb9fff9 282 */
zollecy1 5:62994bb9fff9 283 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];
zollecy1 5:62994bb9fff9 284 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];
zollecy1 5:62994bb9fff9 285 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];
zollecy1 2:4cccdc792719 286 }