Cyrill Zoller / Mbed 2 deprecated calculate

calculate

Dependencies:   mbed X_NUCLEO_IKS01A3 Mahony_Algorithm

CalculateData.cpp@2:4cccdc792719, 2020-04-15 (annotated)

Committer:
zollecy1
Date:
Wed Apr 15 12:13:15 2020 +0000
Revision:
2:4cccdc792719
Parent:
1:48e219526d0f
Child:
3:795998b31c32
.

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 static XNucleoIKS01A3 *mems_expansion_board; //Sensor-Board
zollecy1 1:48e219526d0f 35 static LSM6DSOSensor *acc_gyro; //Gyro-Sensor
zollecy1 1:48e219526d0f 36 static LIS2DW12Sensor *accelerometer; //Acceleroation-Sensor
zollecy1 2:4cccdc792719 37 const int FILTERSIZE = 28;
zollecy1 1:48e219526d0f 38 static int acc_filter[3][FILTERSIZE];
zollecy1 1:48e219526d0f 39 static int gyro_filter[3][FILTERSIZE];
zollecy1 2:4cccdc792719 40 static int speed_filter[3][FILTERSIZE];
zollecy1 2:4cccdc792719 41 const double filter_koef[FILTERSIZE] = {0.04002232346037, -0.1345598020407, -0.05881026752833, -0.03718019471321,
zollecy1 2:4cccdc792719 42 -0.02581625686635, -0.01315040166028, 0.003361386671892, 0.0234601656009,
zollecy1 2:4cccdc792719 43 0.04581625763125, 0.06897560043795, 0.0905455051097, 0.1088826077838,
zollecy1 2:4cccdc792719 44 0.1221264326899, 0.1290832392388, 0.1290832392388, 0.1221264326899,
zollecy1 2:4cccdc792719 45 0.1088826077838, 0.0905455051097, 0.06897560043795, 0.04581625763125,
zollecy1 2:4cccdc792719 46 0.0234601656009, 0.003361386671892, -0.01315040166028, -0.02581625686635,
zollecy1 2:4cccdc792719 47 -0.03718019471321, -0.05881026752833, -0.1345598020407, 0.04002232346037
zollecy1 2:4cccdc792719 48 };
zollecy1 2:4cccdc792719 49
zollecy1 0:313fbc3a198a 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 1:48e219526d0f 87 pos[i]=0;
zollecy1 1:48e219526d0f 88 }
zollecy1 1:48e219526d0f 89
zollecy1 1:48e219526d0f 90 t = 0;
zollecy1 1:48e219526d0f 91 t_old = 0;
zollecy1 1:48e219526d0f 92
zollecy1 1:48e219526d0f 93 //Timer und Ticker starten
zollecy1 1:48e219526d0f 94 timer.start();
zollecy1 2:4cccdc792719 95 ticker.attach(callback(this, &CalculateData::run), PERIOD);
zollecy1 0:313fbc3a198a 96 }
zollecy1 0:313fbc3a198a 97
zollecy1 1:48e219526d0f 98 //Timer und Ticker beenden
zollecy1 1:48e219526d0f 99 void CalculateData::disable(){
zollecy1 2:4cccdc792719 100 //ticker.detach();
zollecy1 1:48e219526d0f 101 timer.stop();
zollecy1 0:313fbc3a198a 102 }
zollecy1 0:313fbc3a198a 103
zollecy1 0:313fbc3a198a 104
zollecy1 0:313fbc3a198a 105
zollecy1 0:313fbc3a198a 106
zollecy1 0:313fbc3a198a 107
zollecy1 1:48e219526d0f 108 //Periodischer Task
zollecy1 1:48e219526d0f 109 void CalculateData::run() {
zollecy1 1:48e219526d0f 110 //Letzte Werte speichern
zollecy1 1:48e219526d0f 111 for (int i=0; i<3;i++){
zollecy1 1:48e219526d0f 112 acc_old[i] = acc[i];
zollecy1 1:48e219526d0f 113 gyro_old[i] = gyro[i];
zollecy1 1:48e219526d0f 114 speed_old[i] = speed[i];
zollecy1 1:48e219526d0f 115 }
zollecy1 1:48e219526d0f 116 t_old = t;
zollecy1 1:48e219526d0f 117
zollecy1 1:48e219526d0f 118 //Sensoren und Timer auslesen
zollecy1 1:48e219526d0f 119 accelerometer->get_x_axes(acc);
zollecy1 1:48e219526d0f 120 acc_gyro->get_g_axes(gyro);
zollecy1 1:48e219526d0f 121 t=timer.read_ms();
zollecy1 1:48e219526d0f 122
zollecy1 2:4cccdc792719 123 filter(acc, 1); //Daten Filtern
zollecy1 2:4cccdc792719 124 filter(gyro, 2); //Daten Filtern
zollecy1 1:48e219526d0f 125 integrate(gyro, gyro_old, angle, t, t_old); //Integral -> Winkel
zollecy1 2:4cccdc792719 126 //transform(acc, angle); //Beschleunigung in Globale Koordinaten transformieren
zollecy1 1:48e219526d0f 127 integrate(acc, acc_old, speed, t, t_old); //Integral -> Geschwindigkeit
zollecy1 2:4cccdc792719 128 filter(speed, 3); //Daten Filtern
zollecy1 1:48e219526d0f 129 integrate(speed, speed_old, pos, t, t_old); //Integral -> Position
zollecy1 1:48e219526d0f 130 }
zollecy1 1:48e219526d0f 131
zollecy1 1:48e219526d0f 132
zollecy1 1:48e219526d0f 133 //Daten Integrieren nach expl. Euler
zollecy1 1:48e219526d0f 134 void CalculateData::integrate(int *x, int *x_old, int *y, int t, int t_old){
zollecy1 1:48e219526d0f 135 int dt = t-t_old;
zollecy1 1:48e219526d0f 136
zollecy1 1:48e219526d0f 137 for(int i = 0;i < 3;i++){
zollecy1 2:4cccdc792719 138 y[i] = (uint32_t)((double)y[i] + ((double)(x[i] + (double)x_old[i])/2.0)*dt/1000.0);
zollecy1 1:48e219526d0f 139 }
zollecy1 1:48e219526d0f 140
zollecy1 1:48e219526d0f 141 }
zollecy1 1:48e219526d0f 142
zollecy1 2:4cccdc792719 143
zollecy1 1:48e219526d0f 144
zollecy1 1:48e219526d0f 145
zollecy1 1:48e219526d0f 146 //Filtern der Daten mittels FIR-Filter
zollecy1 2:4cccdc792719 147 void CalculateData::filter(int *array, int type){
zollecy1 2:4cccdc792719 148
zollecy1 2:4cccdc792719 149 double sum_array[] = {0, 0, 0};
zollecy1 1:48e219526d0f 150
zollecy1 2:4cccdc792719 151 switch(type){
zollecy1 2:4cccdc792719 152
zollecy1 2:4cccdc792719 153 case 1:
zollecy1 2:4cccdc792719 154 for(int j=0;j<3;j++){
zollecy1 2:4cccdc792719 155 for(int i=0;i<FILTERSIZE-2;i++){
zollecy1 2:4cccdc792719 156 acc_filter[j][i] = acc_filter[j][i+1];
zollecy1 2:4cccdc792719 157 sum_array[j] += (filter_koef[i] * (double)acc_filter[j][i]);
zollecy1 2:4cccdc792719 158 }
zollecy1 2:4cccdc792719 159 acc_filter[j][FILTERSIZE-1] = array[j];
zollecy1 2:4cccdc792719 160 sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)acc_filter[j][FILTERSIZE-1]);
zollecy1 2:4cccdc792719 161 array[j] = (int)sum_array[j];
zollecy1 2:4cccdc792719 162 }
zollecy1 2:4cccdc792719 163 break;
zollecy1 1:48e219526d0f 164
zollecy1 2:4cccdc792719 165 case 2:
zollecy1 2:4cccdc792719 166 for(int j=0;j<3;j++){
zollecy1 2:4cccdc792719 167 for(int i=0;i<FILTERSIZE-2;i++){
zollecy1 2:4cccdc792719 168 gyro_filter[j][i] = gyro_filter[j][i+1];
zollecy1 2:4cccdc792719 169 sum_array[j] += (filter_koef[i] * (double)gyro_filter[j][i]);
zollecy1 2:4cccdc792719 170 }
zollecy1 2:4cccdc792719 171 gyro_filter[j][FILTERSIZE-1] = array[j];
zollecy1 2:4cccdc792719 172 sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)gyro_filter[j][FILTERSIZE-1]);
zollecy1 2:4cccdc792719 173 array[j] = (int)sum_array[j];
zollecy1 2:4cccdc792719 174 }
zollecy1 2:4cccdc792719 175 break;
zollecy1 1:48e219526d0f 176
zollecy1 2:4cccdc792719 177 case 3:
zollecy1 2:4cccdc792719 178 for(int j=0;j<3;j++){
zollecy1 2:4cccdc792719 179 for(int i=0;i<FILTERSIZE-2;i++){
zollecy1 2:4cccdc792719 180 speed_filter[j][i] = speed_filter[j][i+1];
zollecy1 2:4cccdc792719 181 sum_array[j] += (filter_koef[i] * (double)speed_filter[j][i]);
zollecy1 2:4cccdc792719 182 }
zollecy1 2:4cccdc792719 183 speed_filter[j][FILTERSIZE-1] = array[j];
zollecy1 2:4cccdc792719 184 sum_array[j] += (filter_koef[FILTERSIZE-1] * (double)speed_filter[j][FILTERSIZE-1]);
zollecy1 2:4cccdc792719 185 array[j] = (int)sum_array[j];
zollecy1 2:4cccdc792719 186 }
zollecy1 2:4cccdc792719 187 default:
zollecy1 2:4cccdc792719 188 break;
zollecy1 2:4cccdc792719 189 }
zollecy1 1:48e219526d0f 190 }
zollecy1 0:313fbc3a198a 191
zollecy1 0:313fbc3a198a 192
zollecy1 0:313fbc3a198a 193
zollecy1 0:313fbc3a198a 194
zollecy1 1:48e219526d0f 195 void CalculateData::getAccelerometer(int *array){
zollecy1 1:48e219526d0f 196 for(int i=0;i<3;i++){
zollecy1 1:48e219526d0f 197 array[i] = acc[i];
zollecy1 1:48e219526d0f 198 }
zollecy1 1:48e219526d0f 199 }
zollecy1 1:48e219526d0f 200
zollecy1 1:48e219526d0f 201
zollecy1 1:48e219526d0f 202 void CalculateData::getSpeed(int *array){
zollecy1 1:48e219526d0f 203 for(int i=0;i<3;i++){
zollecy1 1:48e219526d0f 204 array[i] = speed[i];
zollecy1 1:48e219526d0f 205 }
zollecy1 1:48e219526d0f 206 }
zollecy1 0:313fbc3a198a 207
zollecy1 0:313fbc3a198a 208
zollecy1 1:48e219526d0f 209 void CalculateData::getPosition(int *array){
zollecy1 1:48e219526d0f 210 for(int i=0;i<3;i++){
zollecy1 1:48e219526d0f 211 array[i] = pos[i];
zollecy1 1:48e219526d0f 212 }
zollecy1 1:48e219526d0f 213 }
zollecy1 1:48e219526d0f 214
zollecy1 1:48e219526d0f 215
zollecy1 1:48e219526d0f 216 void CalculateData::getGyro(int *array){
zollecy1 1:48e219526d0f 217 for(int i=0;i<3;i++){
zollecy1 1:48e219526d0f 218 array[i] = gyro[i];
zollecy1 1:48e219526d0f 219 }
zollecy1 1:48e219526d0f 220 }
zollecy1 1:48e219526d0f 221
zollecy1 1:48e219526d0f 222
zollecy1 1:48e219526d0f 223 void CalculateData::getAngle(int *array){
zollecy1 1:48e219526d0f 224 for(int i=0;i<3;i++){
zollecy1 1:48e219526d0f 225 array[i] = angle[i];
zollecy1 1:48e219526d0f 226 }
zollecy1 2:4cccdc792719 227 }
zollecy1 2:4cccdc792719 228
zollecy1 2:4cccdc792719 229
zollecy1 2:4cccdc792719 230 /*
zollecy1 2:4cccdc792719 231 //Beschleunigung in globale Koordinaten Transformieren
zollecy1 2:4cccdc792719 232 void CalculateData::transform(int *acc, int *angle){
zollecy1 2:4cccdc792719 233 double acc_new[3]; //Neue Werte
zollecy1 2:4cccdc792719 234 double a = angle[0]*PI/(1000.0*180.0); //Winkel alpha in rad
zollecy1 2:4cccdc792719 235 double b = angle[1]*PI/(1000.0*180.0); //Winkel beta in rad
zollecy1 2:4cccdc792719 236 double c = angle[2]*PI/(1000.0*180.0); //Winkel gamma in rad
zollecy1 2:4cccdc792719 237
zollecy1 2:4cccdc792719 238 //Transformation
zollecy1 2:4cccdc792719 239 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 240 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 241 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 242
zollecy1 2:4cccdc792719 243 for(int i=0;i<3;i++){
zollecy1 2:4cccdc792719 244 acc[i] = (int)acc_new[i];
zollecy1 2:4cccdc792719 245 }
zollecy1 2:4cccdc792719 246 }
zollecy1 2:4cccdc792719 247 */