Roll & Pitch Angles (Kalman Filter)
Dependencies: L3GD20H LSM303DLHC kalman mbed-dsp mbed-rtos mbed
Diff: main.cpp
- Revision:
- 0:7fd305c81a8e
- Child:
- 2:f3043132a959
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Apr 09 04:27:35 2016 +0000 @@ -0,0 +1,136 @@ +#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; +} \ No newline at end of file