Roll & Pitch Angles (Kalman Filter)
Dependencies: L3GD20H LSM303DLHC kalman mbed-dsp mbed-rtos mbed
Revision 3:874424bbe577, committed 2016-04-13
- Comitter:
- julioefajardo
- Date:
- Wed Apr 13 21:11:55 2016 +0000
- Parent:
- 2:f3043132a959
- Commit message:
- Version Updated 13/apr/16
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Apr 09 19:17:26 2016 +0000 +++ b/main.cpp Wed Apr 13 21:11:55 2016 +0000 @@ -7,17 +7,21 @@ #define Rad2Dree 57.295779513082320876798154814105f -#define PID_L_KP 0.0275f /* Proporcional */ //0.015f -#define PID_L_KI 0.000125f /* Integral */ -#define PID_L_KD 0.075f /* Derivative */ +#define PID_L_KP 0.0275f /* Proporcional */ //0.015f +#define PID_L_KI 0.0f /* Integral */ +#define PID_L_KD 0.0f /* Derivative */ #define PID_R_KP 0.0275f /* Proporcional */ //0.015f -#define PID_R_KI 0.000125f /* Integral */ -#define PID_R_KD 0.075f /* Derivative */ +#define PID_R_KI 0.0f /* Integral */ +#define PID_R_KD 0.0f /* Derivative */ -#define L_SP PI/2 +#define L_SP PI/2 #define R_SP PI/2 +#define YOFF 100.0 // Y-Offset +#define XOFF 2.0 // X-Offset +#define WOFF 12.0 // Winkel-Offset + DigitalOut led_red(LED_RED); DigitalOut led_green(LED_GREEN); DigitalIn sw2(SW2); @@ -30,6 +34,11 @@ LSM303DLHC imuR(PTC11,PTC10); L3GD20H gyroR(PTC11,PTC10); +PwmOut M1(D13); +PwmOut M2(D12); +DigitalOut M1D(D11); +DigitalOut M2D(D10); + Timer GlobalTime; Timer ProgramTimer; @@ -70,6 +79,7 @@ void GyrRaw2DL(short gyr[3]); void AccRaw2GR(int acc[3]); void GyrRaw2DR(short gyr[3]); +double calcHeading(int *mag); int main() { arm_pid_instance_f32 L_PID; @@ -91,10 +101,16 @@ GlobalTime.start(); imuL.init(); imuR.init(); + M1.period(1.0f/1000.0f); //Comparten el mismo timer + M1.pulsewidth(0.0f/1000.0f); + M2.pulsewidth(0.0f/1000.0f); + led_green = 1; led_red = 1; + M1D = 0; + M2D = 0; pc.baud(115200); - pc.printf("Hello World from FRDM-K64F board.\r\n"); + kalman_init(&filter_pitch_L, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); kalman_init(&filter_roll_L, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); kalman_init(&filter_pitch_R, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); @@ -106,11 +122,13 @@ while (true) { imuL.readAcc(accL); + imuL.readMag(magL); AccRaw2GL(accL); gyroL.read(gyrL); GyrRaw2DL(gyrL); imuR.readAcc(accR); + imuR.readMag(magR); AccRaw2GR(accR); gyroR.read(gyrR); GyrRaw2DR(gyrR); @@ -134,6 +152,9 @@ angleR[0] = kalman_get_angle(&filter_pitch_R); angleR[1] = kalman_get_angle(&filter_roll_R); + angleL[2] = calcHeading(magL); + angleR[2] = calcHeading(magR); + L_error = angleL[0] - L_SP; R_error = angleR[0] - R_SP; @@ -142,8 +163,8 @@ timer = ProgramTimer.read_us(); - printf("IMUL\tPitch=%6.2f\tRoll=%6.2f\r\n",Rad2Dree*angleL[0],Rad2Dree*angleL[1]); - printf("IMIR\tPitch=%6.2f\tRoll=%6.2f\r\n",Rad2Dree*angleR[0],Rad2Dree*angleR[1]); + printf("IMUL\tPitch=%6.2f\tRoll=%6.2f\tYaw=%6.2f\r\n",Rad2Dree*angleL[0],Rad2Dree*angleL[1],angleL[2]); + //printf("IMIR\tPitch=%6.2f\tRoll=%6.2f\tYaw=%6.2f\r\n",Rad2Dree*angleR[0],Rad2Dree*angleR[1],angleR[2]); wait(0.2); } } @@ -170,4 +191,30 @@ GyrR.x = gyr[0]/225.0f; GyrR.y = gyr[1]/225.0f; GyrR.z = gyr[2]/225.0f; +} + +double calcHeading(int *mag) +{ + double x,y; + double hdg; + + x = mag[0] + XOFF; // X-Achsen Ausgleich + y = mag[1] + YOFF; // Y-Achsen Ausgleich + + hdg = atan(y/x); + hdg *= Rad2Dree; // Umrechnung von Bogen- nach Winkelmass + if (x > 0) + { + if(y>0) hdg = hdg; // Korrektur 1. Quadrant + else hdg = 360.0 + hdg; // Korrektur 4. Quadrant + } + else + { + if(y>0) hdg = 180.0 + hdg; // Korrektur 2. Quadrant + else hdg = 180.0 + hdg; // Korrektur 3. Quadrant + } + + hdg -= WOFF; // Korrektur Winkel (Vorsicht: Bei Winkeln < WOFF wird hdg negativ!) + if (hdg <0) hdg = 360.0 + hdg; // Winkel soll nicht negativ werden! (Bsp.: -5° => 355°) + return (hdg); } \ No newline at end of file