Julio Fajardo / Mbed 2 deprecated Galileo_HoverBoardRider

Roll & Pitch Angles (Kalman Filter)

Dependencies:   L3GD20H LSM303DLHC kalman mbed-dsp mbed-rtos mbed

main.cpp@3:874424bbe577, 2016-04-13 (annotated)

Committer:
julioefajardo
Date:
Wed Apr 13 21:11:55 2016 +0000
Revision:
3:874424bbe577
Parent:
2:f3043132a959 
Version Updated 13/apr/16

Who changed what in which revision?

UserRevisionLine numberNew contents of line
julioefajardo 0:7fd305c81a8e 1 #include "mbed.h"
julioefajardo 0:7fd305c81a8e 2 #include "rtos.h"
julioefajardo 0:7fd305c81a8e 3 #include "arm_math.h"
julioefajardo 0:7fd305c81a8e 4 #include "kalman.c"
julioefajardo 0:7fd305c81a8e 5 #include "LSM303DLHC.h"
julioefajardo 0:7fd305c81a8e 6 #include "L3GD20H.h"
julioefajardo 0:7fd305c81a8e 7
julioefajardo 0:7fd305c81a8e 8 #define Rad2Dree 57.295779513082320876798154814105f
julioefajardo 0:7fd305c81a8e 9
julioefajardo 3:874424bbe577 10 #define PID_L_KP 0.0275f /* Proporcional */ //0.015f
julioefajardo 3:874424bbe577 11 #define PID_L_KI 0.0f /* Integral */
julioefajardo 3:874424bbe577 12 #define PID_L_KD 0.0f /* Derivative */
julioefajardo 2:f3043132a959 13
julioefajardo 2:f3043132a959 14 #define PID_R_KP 0.0275f /* Proporcional */ //0.015f
julioefajardo 3:874424bbe577 15 #define PID_R_KI 0.0f /* Integral */
julioefajardo 3:874424bbe577 16 #define PID_R_KD 0.0f /* Derivative */
julioefajardo 2:f3043132a959 17
julioefajardo 3:874424bbe577 18 #define L_SP PI/2
julioefajardo 2:f3043132a959 19 #define R_SP PI/2
julioefajardo 2:f3043132a959 20
julioefajardo 3:874424bbe577 21 #define YOFF 100.0 // Y-Offset
julioefajardo 3:874424bbe577 22 #define XOFF 2.0 // X-Offset
julioefajardo 3:874424bbe577 23 #define WOFF 12.0 // Winkel-Offset
julioefajardo 3:874424bbe577 24
julioefajardo 0:7fd305c81a8e 25 DigitalOut led_red(LED_RED);
julioefajardo 0:7fd305c81a8e 26 DigitalOut led_green(LED_GREEN);
julioefajardo 0:7fd305c81a8e 27 DigitalIn sw2(SW2);
julioefajardo 0:7fd305c81a8e 28 DigitalIn sw3(SW3);
julioefajardo 0:7fd305c81a8e 29 Serial pc(USBTX, USBRX);
julioefajardo 0:7fd305c81a8e 30
julioefajardo 2:f3043132a959 31 LSM303DLHC imuL(D14,D15);
julioefajardo 2:f3043132a959 32 L3GD20H gyroL(D14,D15);
julioefajardo 0:7fd305c81a8e 33
julioefajardo 2:f3043132a959 34 LSM303DLHC imuR(PTC11,PTC10);
julioefajardo 2:f3043132a959 35 L3GD20H gyroR(PTC11,PTC10);
julioefajardo 0:7fd305c81a8e 36
julioefajardo 3:874424bbe577 37 PwmOut M1(D13);
julioefajardo 3:874424bbe577 38 PwmOut M2(D12);
julioefajardo 3:874424bbe577 39 DigitalOut M1D(D11);
julioefajardo 3:874424bbe577 40 DigitalOut M2D(D10);
julioefajardo 3:874424bbe577 41
julioefajardo 0:7fd305c81a8e 42 Timer GlobalTime;
julioefajardo 0:7fd305c81a8e 43 Timer ProgramTimer;
julioefajardo 0:7fd305c81a8e 44
julioefajardo 2:f3043132a959 45 kalman filter_pitch_L;
julioefajardo 2:f3043132a959 46 kalman filter_roll_L;
julioefajardo 0:7fd305c81a8e 47
julioefajardo 2:f3043132a959 48 kalman filter_pitch_R;
julioefajardo 2:f3043132a959 49 kalman filter_roll_R;
julioefajardo 0:7fd305c81a8e 50
julioefajardo 2:f3043132a959 51 int accL[3];
julioefajardo 2:f3043132a959 52 int magL[3];
julioefajardo 2:f3043132a959 53 short gyrL[3];
julioefajardo 0:7fd305c81a8e 54
julioefajardo 2:f3043132a959 55 int accR[3];
julioefajardo 2:f3043132a959 56 int magR[3];
julioefajardo 2:f3043132a959 57 short gyrR[3];
julioefajardo 0:7fd305c81a8e 58
julioefajardo 0:7fd305c81a8e 59 struct vector {
julioefajardo 0:7fd305c81a8e 60 float x;
julioefajardo 0:7fd305c81a8e 61 float y;
julioefajardo 0:7fd305c81a8e 62 float z;
julioefajardo 2:f3043132a959 63 } AccL, GyrL, AccR, GyrR;
julioefajardo 0:7fd305c81a8e 64
julioefajardo 2:f3043132a959 65 float RL, RR;
julioefajardo 2:f3043132a959 66 double angleL[3];
julioefajardo 2:f3043132a959 67 double angleR[3];
julioefajardo 2:f3043132a959 68
julioefajardo 2:f3043132a959 69 float L_error;
julioefajardo 2:f3043132a959 70 float R_error;
julioefajardo 2:f3043132a959 71
julioefajardo 2:f3043132a959 72 float L;
julioefajardo 0:7fd305c81a8e 73 float R;
julioefajardo 0:7fd305c81a8e 74
julioefajardo 0:7fd305c81a8e 75 unsigned long timer;
julioefajardo 0:7fd305c81a8e 76 long loopStartTime;
julioefajardo 0:7fd305c81a8e 77
julioefajardo 2:f3043132a959 78 void AccRaw2GL(int acc[3]);
julioefajardo 2:f3043132a959 79 void GyrRaw2DL(short gyr[3]);
julioefajardo 2:f3043132a959 80 void AccRaw2GR(int acc[3]);
julioefajardo 2:f3043132a959 81 void GyrRaw2DR(short gyr[3]);
julioefajardo 3:874424bbe577 82 double calcHeading(int *mag);
julioefajardo 0:7fd305c81a8e 83
julioefajardo 0:7fd305c81a8e 84 int main() {
julioefajardo 2:f3043132a959 85 arm_pid_instance_f32 L_PID;
julioefajardo 2:f3043132a959 86 arm_pid_instance_f32 R_PID;
julioefajardo 2:f3043132a959 87
julioefajardo 2:f3043132a959 88 //Left
julioefajardo 2:f3043132a959 89 L_PID.Kp = PID_L_KP; /* Proporcional */
julioefajardo 2:f3043132a959 90 L_PID.Ki = PID_L_KI; /* Integral */
julioefajardo 2:f3043132a959 91 L_PID.Kd = PID_L_KD; /* Derivative */
julioefajardo 2:f3043132a959 92
julioefajardo 2:f3043132a959 93 //Right
julioefajardo 2:f3043132a959 94 R_PID.Kp = PID_R_KP; /* Proporcional */
julioefajardo 2:f3043132a959 95 R_PID.Ki = PID_R_KI; /* Integral */
julioefajardo 2:f3043132a959 96 R_PID.Kd = PID_R_KD; /* Derivative */
julioefajardo 2:f3043132a959 97
julioefajardo 2:f3043132a959 98 arm_pid_init_f32(&L_PID, 1);
julioefajardo 2:f3043132a959 99 arm_pid_init_f32(&R_PID, 1);
julioefajardo 2:f3043132a959 100
julioefajardo 0:7fd305c81a8e 101 GlobalTime.start();
julioefajardo 2:f3043132a959 102 imuL.init();
julioefajardo 2:f3043132a959 103 imuR.init();
julioefajardo 3:874424bbe577 104 M1.period(1.0f/1000.0f); //Comparten el mismo timer
julioefajardo 3:874424bbe577 105 M1.pulsewidth(0.0f/1000.0f);
julioefajardo 3:874424bbe577 106 M2.pulsewidth(0.0f/1000.0f);
julioefajardo 3:874424bbe577 107
julioefajardo 0:7fd305c81a8e 108 led_green = 1;
julioefajardo 0:7fd305c81a8e 109 led_red = 1;
julioefajardo 3:874424bbe577 110 M1D = 0;
julioefajardo 3:874424bbe577 111 M2D = 0;
julioefajardo 0:7fd305c81a8e 112 pc.baud(115200);
julioefajardo 3:874424bbe577 113
julioefajardo 2:f3043132a959 114 kalman_init(&filter_pitch_L, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 2:f3043132a959 115 kalman_init(&filter_roll_L, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 2:f3043132a959 116 kalman_init(&filter_pitch_R, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 2:f3043132a959 117 kalman_init(&filter_roll_R, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 0:7fd305c81a8e 118
julioefajardo 0:7fd305c81a8e 119 ProgramTimer.start();
julioefajardo 0:7fd305c81a8e 120 loopStartTime = ProgramTimer.read_us();
julioefajardo 0:7fd305c81a8e 121 timer = loopStartTime;
julioefajardo 0:7fd305c81a8e 122
julioefajardo 0:7fd305c81a8e 123 while (true) {
julioefajardo 2:f3043132a959 124 imuL.readAcc(accL);
julioefajardo 3:874424bbe577 125 imuL.readMag(magL);
julioefajardo 2:f3043132a959 126 AccRaw2GL(accL);
julioefajardo 2:f3043132a959 127 gyroL.read(gyrL);
julioefajardo 2:f3043132a959 128 GyrRaw2DL(gyrL);
julioefajardo 0:7fd305c81a8e 129
julioefajardo 2:f3043132a959 130 imuR.readAcc(accR);
julioefajardo 3:874424bbe577 131 imuR.readMag(magR);
julioefajardo 2:f3043132a959 132 AccRaw2GR(accR);
julioefajardo 2:f3043132a959 133 gyroR.read(gyrR);
julioefajardo 2:f3043132a959 134 GyrRaw2DR(gyrR);
julioefajardo 0:7fd305c81a8e 135
julioefajardo 2:f3043132a959 136 RL = sqrt(std::pow(AccL.x, 2) + std::pow(AccL.y, 2) + std::pow(AccL.z, 2));
julioefajardo 2:f3043132a959 137 RR = sqrt(std::pow(AccR.x, 2) + std::pow(AccR.y, 2) + std::pow(AccR.z, 2));
julioefajardo 0:7fd305c81a8e 138
julioefajardo 2:f3043132a959 139 kalman_predict(&filter_pitch_L, GyrL.x, (ProgramTimer.read_us() - timer));
julioefajardo 2:f3043132a959 140 kalman_update(&filter_pitch_L, acos(AccL.x/RL));
julioefajardo 2:f3043132a959 141 kalman_predict(&filter_roll_L, GyrL.y, (ProgramTimer.read_us() - timer));
julioefajardo 2:f3043132a959 142 kalman_update(&filter_roll_L, acos(AccL.y/RL));
julioefajardo 2:f3043132a959 143
julioefajardo 2:f3043132a959 144 kalman_predict(&filter_pitch_R, GyrR.x, (ProgramTimer.read_us() - timer));
julioefajardo 2:f3043132a959 145 kalman_update(&filter_pitch_R, acos(AccR.x/RR));
julioefajardo 2:f3043132a959 146 kalman_predict(&filter_roll_R, GyrR.y, (ProgramTimer.read_us() - timer));
julioefajardo 2:f3043132a959 147 kalman_update(&filter_roll_R, acos(AccR.y/RR));
julioefajardo 0:7fd305c81a8e 148
julioefajardo 2:f3043132a959 149 angleL[0] = kalman_get_angle(&filter_pitch_L);
julioefajardo 2:f3043132a959 150 angleL[1] = kalman_get_angle(&filter_roll_L);
julioefajardo 2:f3043132a959 151
julioefajardo 2:f3043132a959 152 angleR[0] = kalman_get_angle(&filter_pitch_R);
julioefajardo 2:f3043132a959 153 angleR[1] = kalman_get_angle(&filter_roll_R);
julioefajardo 0:7fd305c81a8e 154
julioefajardo 3:874424bbe577 155 angleL[2] = calcHeading(magL);
julioefajardo 3:874424bbe577 156 angleR[2] = calcHeading(magR);
julioefajardo 3:874424bbe577 157
julioefajardo 2:f3043132a959 158 L_error = angleL[0] - L_SP;
julioefajardo 2:f3043132a959 159 R_error = angleR[0] - R_SP;
julioefajardo 0:7fd305c81a8e 160
julioefajardo 2:f3043132a959 161 L = arm_pid_f32(&L_PID, L_error);
julioefajardo 2:f3043132a959 162 R = arm_pid_f32(&R_PID, R_error);
julioefajardo 0:7fd305c81a8e 163
julioefajardo 0:7fd305c81a8e 164 timer = ProgramTimer.read_us();
julioefajardo 0:7fd305c81a8e 165
julioefajardo 3:874424bbe577 166 printf("IMUL\tPitch=%6.2f\tRoll=%6.2f\tYaw=%6.2f\r\n",Rad2Dree*angleL[0],Rad2Dree*angleL[1],angleL[2]);
julioefajardo 3:874424bbe577 167 //printf("IMIR\tPitch=%6.2f\tRoll=%6.2f\tYaw=%6.2f\r\n",Rad2Dree*angleR[0],Rad2Dree*angleR[1],angleR[2]);
julioefajardo 0:7fd305c81a8e 168 wait(0.2);
julioefajardo 0:7fd305c81a8e 169 }
julioefajardo 0:7fd305c81a8e 170 }
julioefajardo 0:7fd305c81a8e 171
julioefajardo 2:f3043132a959 172 void AccRaw2GL(int acc[3]){
julioefajardo 2:f3043132a959 173 AccL.x = acc[0]/1024.0f;
julioefajardo 2:f3043132a959 174 AccL.y = acc[1]/1024.0f;
julioefajardo 2:f3043132a959 175 AccL.z = acc[2]/1024.0f;
julioefajardo 0:7fd305c81a8e 176 }
julioefajardo 0:7fd305c81a8e 177
julioefajardo 2:f3043132a959 178 void GyrRaw2DL(short gyr[3]){
julioefajardo 2:f3043132a959 179 GyrL.x = gyr[0]/225.0f;
julioefajardo 2:f3043132a959 180 GyrL.y = gyr[1]/225.0f;
julioefajardo 2:f3043132a959 181 GyrL.z = gyr[2]/225.0f;
julioefajardo 0:7fd305c81a8e 182 }
julioefajardo 0:7fd305c81a8e 183
julioefajardo 2:f3043132a959 184 void AccRaw2GR(int acc[3]){
julioefajardo 2:f3043132a959 185 AccR.x = acc[0]/1024.0f;
julioefajardo 2:f3043132a959 186 AccR.y = acc[1]/1024.0f;
julioefajardo 2:f3043132a959 187 AccR.z = acc[2]/1024.0f;
julioefajardo 0:7fd305c81a8e 188 }
julioefajardo 0:7fd305c81a8e 189
julioefajardo 2:f3043132a959 190 void GyrRaw2DR(short gyr[3]){
julioefajardo 2:f3043132a959 191 GyrR.x = gyr[0]/225.0f;
julioefajardo 2:f3043132a959 192 GyrR.y = gyr[1]/225.0f;
julioefajardo 2:f3043132a959 193 GyrR.z = gyr[2]/225.0f;
julioefajardo 3:874424bbe577 194 }
julioefajardo 3:874424bbe577 195
julioefajardo 3:874424bbe577 196 double calcHeading(int *mag)
julioefajardo 3:874424bbe577 197 {
julioefajardo 3:874424bbe577 198 double x,y;
julioefajardo 3:874424bbe577 199 double hdg;
julioefajardo 3:874424bbe577 200
julioefajardo 3:874424bbe577 201 x = mag[0] + XOFF; // X-Achsen Ausgleich
julioefajardo 3:874424bbe577 202 y = mag[1] + YOFF; // Y-Achsen Ausgleich
julioefajardo 3:874424bbe577 203
julioefajardo 3:874424bbe577 204 hdg = atan(y/x);
julioefajardo 3:874424bbe577 205 hdg *= Rad2Dree; // Umrechnung von Bogen- nach Winkelmass
julioefajardo 3:874424bbe577 206 if (x > 0)
julioefajardo 3:874424bbe577 207 {
julioefajardo 3:874424bbe577 208 if(y>0) hdg = hdg; // Korrektur 1. Quadrant
julioefajardo 3:874424bbe577 209 else hdg = 360.0 + hdg; // Korrektur 4. Quadrant
julioefajardo 3:874424bbe577 210 }
julioefajardo 3:874424bbe577 211 else
julioefajardo 3:874424bbe577 212 {
julioefajardo 3:874424bbe577 213 if(y>0) hdg = 180.0 + hdg; // Korrektur 2. Quadrant
julioefajardo 3:874424bbe577 214 else hdg = 180.0 + hdg; // Korrektur 3. Quadrant
julioefajardo 3:874424bbe577 215 }
julioefajardo 3:874424bbe577 216
julioefajardo 3:874424bbe577 217 hdg -= WOFF; // Korrektur Winkel (Vorsicht: Bei Winkeln < WOFF wird hdg negativ!)
julioefajardo 3:874424bbe577 218 if (hdg <0) hdg = 360.0 + hdg; // Winkel soll nicht negativ werden! (Bsp.: -5° => 355°)
julioefajardo 3:874424bbe577 219 return (hdg);
julioefajardo 0:7fd305c81a8e 220 }