Roll & Pitch Angles (Kalman Filter)
Dependencies: L3GD20H LSM303DLHC kalman mbed-dsp mbed-rtos mbed
main.cpp
- Committer:
- julioefajardo
- Date:
- 2016-04-09
- Revision:
- 0:7fd305c81a8e
- Child:
- 2:f3043132a959
File content as of revision 0:7fd305c81a8e:
#include "mbed.h" #include "rtos.h" #include "arm_math.h" #include "kalman.c" #include "LSM303DLHC.h" #include "L3GD20H.h" #define Rad2Dree 57.295779513082320876798154814105f DigitalOut led_red(LED_RED); DigitalOut led_green(LED_GREEN); DigitalIn sw2(SW2); DigitalIn sw3(SW3); Serial pc(USBTX, USBRX); LSM303DLHC imu(D14,D15); L3GD20H gyro(D14,D15); LSM303DLHC imu2(PTC11,PTC10); L3GD20H gyro2(PTC11,PTC10); Timer GlobalTime; Timer ProgramTimer; kalman filter_pitch; kalman filter_roll; kalman filter_pitch2; kalman filter_roll2; int acc[3]; int mag[3]; short gyr[3]; int acc2[3]; int mag2[3]; short gyr2[3]; struct vector { float x; float y; float z; } Acc, Gyr, Acc2, Gyr2; float R; double angle[3]; float R2; double angle2[3]; unsigned long timer; long loopStartTime; void AccRaw2G(int acc[3]); void GyrRaw2D(short gyr[3]); void AccRaw2G2(int acc[3]); void GyrRaw2D2(short gyr[3]); int main() { GlobalTime.start(); imu.init(); imu2.init(); led_green = 1; led_red = 1; pc.baud(115200); pc.printf("Hello World from FRDM-K64F board.\r\n"); kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); kalman_init(&filter_pitch2, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); kalman_init(&filter_roll2, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); ProgramTimer.start(); loopStartTime = ProgramTimer.read_us(); timer = loopStartTime; while (true) { imu.readAcc(acc); AccRaw2G(acc); gyro.read(gyr); GyrRaw2D(gyr); imu2.readAcc(acc2); AccRaw2G2(acc2); gyro2.read(gyr2); GyrRaw2D2(gyr2); R = sqrt(std::pow(Acc.x, 2) + std::pow(Acc.y, 2) + std::pow(Acc.z, 2)); R2 = sqrt(std::pow(Acc2.x, 2) + std::pow(Acc2.y, 2) + std::pow(Acc2.z, 2)); kalman_predict(&filter_pitch, Gyr.x, (ProgramTimer.read_us() - timer)); kalman_update(&filter_pitch, acos(Acc.x/R)); kalman_predict(&filter_roll, Gyr.y, (ProgramTimer.read_us() - timer)); kalman_update(&filter_roll, acos(Acc.y/R)); kalman_predict(&filter_pitch2, Gyr2.x, (ProgramTimer.read_us() - timer)); kalman_update(&filter_pitch2, acos(Acc2.x/R2)); kalman_predict(&filter_roll2, Gyr.y, (ProgramTimer.read_us() - timer)); kalman_update(&filter_roll2, acos(Acc2.y/R2)); angle[0] = kalman_get_angle(&filter_pitch); angle[1] = kalman_get_angle(&filter_roll); angle2[0] = kalman_get_angle(&filter_pitch2); angle2[1] = kalman_get_angle(&filter_roll2); timer = ProgramTimer.read_us(); printf("IMU1\tangle0=%6.2f\tangle1=%6.2f\r\n",Rad2Dree*angle[0],Rad2Dree*angle[1]); printf("IMI2\tangle0=%6.2f\tangle1=%6.2f\r\n",Rad2Dree*angle2[0],Rad2Dree*angle2[1]); wait(0.2); } } void AccRaw2G(int acc[3]){ Acc.x = acc[0]/1024.0f; Acc.y = acc[1]/1024.0f; Acc.z = acc[2]/1024.0f; } void GyrRaw2D(short gyr[3]){ Gyr.x = gyr[0]/225.0f; Gyr.y = gyr[1]/225.0f; Gyr.z = gyr[2]/225.0f; } void AccRaw2G2(int acc[3]){ Acc2.x = acc[0]/1024.0f; Acc2.y = acc[1]/1024.0f; Acc2.z = acc[2]/1024.0f; } void GyrRaw2D2(short gyr[3]){ Gyr2.x = gyr[0]/225.0f; Gyr2.y = gyr[1]/225.0f; Gyr2.z = gyr[2]/225.0f; }