Cyrill Zoller / Mbed 2 deprecated calculate

calculate

Dependencies:   mbed X_NUCLEO_IKS01A3 Mahony_Algorithm

CalculateData.cpp@4:7d13076ecece, 2020-04-24 (annotated)

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