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?
User | Revision | Line number | New 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 | */ |