Roll & Pitch Angles (Kalman Filter)
Dependencies: L3GD20H LSM303DLHC kalman mbed-dsp mbed-rtos mbed
main.cpp
- Committer:
- julioefajardo
- Date:
- 2016-04-13
- Revision:
- 3:874424bbe577
- Parent:
- 2:f3043132a959
File content as of revision 3:874424bbe577:
#include "mbed.h" #include "rtos.h" #include "arm_math.h" #include "kalman.c" #include "LSM303DLHC.h" #include "L3GD20H.h" #define Rad2Dree 57.295779513082320876798154814105f #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.0f /* Integral */ #define PID_R_KD 0.0f /* Derivative */ #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); DigitalIn sw3(SW3); Serial pc(USBTX, USBRX); LSM303DLHC imuL(D14,D15); L3GD20H gyroL(D14,D15); LSM303DLHC imuR(PTC11,PTC10); L3GD20H gyroR(PTC11,PTC10); PwmOut M1(D13); PwmOut M2(D12); DigitalOut M1D(D11); DigitalOut M2D(D10); Timer GlobalTime; Timer ProgramTimer; kalman filter_pitch_L; kalman filter_roll_L; kalman filter_pitch_R; kalman filter_roll_R; int accL[3]; int magL[3]; short gyrL[3]; int accR[3]; int magR[3]; short gyrR[3]; struct vector { float x; float y; float z; } AccL, GyrL, AccR, GyrR; float RL, RR; double angleL[3]; double angleR[3]; float L_error; float R_error; float L; float R; unsigned long timer; long loopStartTime; void AccRaw2GL(int acc[3]); 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; arm_pid_instance_f32 R_PID; //Left L_PID.Kp = PID_L_KP; /* Proporcional */ L_PID.Ki = PID_L_KI; /* Integral */ L_PID.Kd = PID_L_KD; /* Derivative */ //Right R_PID.Kp = PID_R_KP; /* Proporcional */ R_PID.Ki = PID_R_KI; /* Integral */ R_PID.Kd = PID_R_KD; /* Derivative */ arm_pid_init_f32(&L_PID, 1); arm_pid_init_f32(&R_PID, 1); 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); 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); kalman_init(&filter_roll_R, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); ProgramTimer.start(); loopStartTime = ProgramTimer.read_us(); timer = loopStartTime; 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); RL = sqrt(std::pow(AccL.x, 2) + std::pow(AccL.y, 2) + std::pow(AccL.z, 2)); RR = sqrt(std::pow(AccR.x, 2) + std::pow(AccR.y, 2) + std::pow(AccR.z, 2)); kalman_predict(&filter_pitch_L, GyrL.x, (ProgramTimer.read_us() - timer)); kalman_update(&filter_pitch_L, acos(AccL.x/RL)); kalman_predict(&filter_roll_L, GyrL.y, (ProgramTimer.read_us() - timer)); kalman_update(&filter_roll_L, acos(AccL.y/RL)); kalman_predict(&filter_pitch_R, GyrR.x, (ProgramTimer.read_us() - timer)); kalman_update(&filter_pitch_R, acos(AccR.x/RR)); kalman_predict(&filter_roll_R, GyrR.y, (ProgramTimer.read_us() - timer)); kalman_update(&filter_roll_R, acos(AccR.y/RR)); angleL[0] = kalman_get_angle(&filter_pitch_L); angleL[1] = kalman_get_angle(&filter_roll_L); 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; L = arm_pid_f32(&L_PID, L_error); R = arm_pid_f32(&R_PID, R_error); timer = ProgramTimer.read_us(); 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); } } void AccRaw2GL(int acc[3]){ AccL.x = acc[0]/1024.0f; AccL.y = acc[1]/1024.0f; AccL.z = acc[2]/1024.0f; } void GyrRaw2DL(short gyr[3]){ GyrL.x = gyr[0]/225.0f; GyrL.y = gyr[1]/225.0f; GyrL.z = gyr[2]/225.0f; } void AccRaw2GR(int acc[3]){ AccR.x = acc[0]/1024.0f; AccR.y = acc[1]/1024.0f; AccR.z = acc[2]/1024.0f; } void GyrRaw2DR(short gyr[3]){ 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); }