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