Roll & Pitch Angles (Kalman Filter)
Dependencies: L3GD20H LSM303DLHC kalman mbed-dsp mbed-rtos mbed
main.cpp@0:7fd305c81a8e, 2016-04-09 (annotated)
- Committer:
- julioefajardo
- Date:
- Sat Apr 09 04:27:35 2016 +0000
- Revision:
- 0:7fd305c81a8e
- Child:
- 2:f3043132a959
Roll & Pith (Kalman Filter)
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 | 0:7fd305c81a8e | 10 | DigitalOut led_red(LED_RED); |
julioefajardo | 0:7fd305c81a8e | 11 | DigitalOut led_green(LED_GREEN); |
julioefajardo | 0:7fd305c81a8e | 12 | DigitalIn sw2(SW2); |
julioefajardo | 0:7fd305c81a8e | 13 | DigitalIn sw3(SW3); |
julioefajardo | 0:7fd305c81a8e | 14 | Serial pc(USBTX, USBRX); |
julioefajardo | 0:7fd305c81a8e | 15 | |
julioefajardo | 0:7fd305c81a8e | 16 | LSM303DLHC imu(D14,D15); |
julioefajardo | 0:7fd305c81a8e | 17 | L3GD20H gyro(D14,D15); |
julioefajardo | 0:7fd305c81a8e | 18 | |
julioefajardo | 0:7fd305c81a8e | 19 | LSM303DLHC imu2(PTC11,PTC10); |
julioefajardo | 0:7fd305c81a8e | 20 | L3GD20H gyro2(PTC11,PTC10); |
julioefajardo | 0:7fd305c81a8e | 21 | |
julioefajardo | 0:7fd305c81a8e | 22 | Timer GlobalTime; |
julioefajardo | 0:7fd305c81a8e | 23 | Timer ProgramTimer; |
julioefajardo | 0:7fd305c81a8e | 24 | |
julioefajardo | 0:7fd305c81a8e | 25 | kalman filter_pitch; |
julioefajardo | 0:7fd305c81a8e | 26 | kalman filter_roll; |
julioefajardo | 0:7fd305c81a8e | 27 | |
julioefajardo | 0:7fd305c81a8e | 28 | kalman filter_pitch2; |
julioefajardo | 0:7fd305c81a8e | 29 | kalman filter_roll2; |
julioefajardo | 0:7fd305c81a8e | 30 | |
julioefajardo | 0:7fd305c81a8e | 31 | int acc[3]; |
julioefajardo | 0:7fd305c81a8e | 32 | int mag[3]; |
julioefajardo | 0:7fd305c81a8e | 33 | short gyr[3]; |
julioefajardo | 0:7fd305c81a8e | 34 | |
julioefajardo | 0:7fd305c81a8e | 35 | int acc2[3]; |
julioefajardo | 0:7fd305c81a8e | 36 | int mag2[3]; |
julioefajardo | 0:7fd305c81a8e | 37 | short gyr2[3]; |
julioefajardo | 0:7fd305c81a8e | 38 | |
julioefajardo | 0:7fd305c81a8e | 39 | struct vector { |
julioefajardo | 0:7fd305c81a8e | 40 | float x; |
julioefajardo | 0:7fd305c81a8e | 41 | float y; |
julioefajardo | 0:7fd305c81a8e | 42 | float z; |
julioefajardo | 0:7fd305c81a8e | 43 | } Acc, Gyr, Acc2, Gyr2; |
julioefajardo | 0:7fd305c81a8e | 44 | |
julioefajardo | 0:7fd305c81a8e | 45 | float R; |
julioefajardo | 0:7fd305c81a8e | 46 | double angle[3]; |
julioefajardo | 0:7fd305c81a8e | 47 | |
julioefajardo | 0:7fd305c81a8e | 48 | float R2; |
julioefajardo | 0:7fd305c81a8e | 49 | double angle2[3]; |
julioefajardo | 0:7fd305c81a8e | 50 | |
julioefajardo | 0:7fd305c81a8e | 51 | unsigned long timer; |
julioefajardo | 0:7fd305c81a8e | 52 | long loopStartTime; |
julioefajardo | 0:7fd305c81a8e | 53 | |
julioefajardo | 0:7fd305c81a8e | 54 | void AccRaw2G(int acc[3]); |
julioefajardo | 0:7fd305c81a8e | 55 | void GyrRaw2D(short gyr[3]); |
julioefajardo | 0:7fd305c81a8e | 56 | void AccRaw2G2(int acc[3]); |
julioefajardo | 0:7fd305c81a8e | 57 | void GyrRaw2D2(short gyr[3]); |
julioefajardo | 0:7fd305c81a8e | 58 | |
julioefajardo | 0:7fd305c81a8e | 59 | int main() { |
julioefajardo | 0:7fd305c81a8e | 60 | GlobalTime.start(); |
julioefajardo | 0:7fd305c81a8e | 61 | imu.init(); |
julioefajardo | 0:7fd305c81a8e | 62 | imu2.init(); |
julioefajardo | 0:7fd305c81a8e | 63 | led_green = 1; |
julioefajardo | 0:7fd305c81a8e | 64 | led_red = 1; |
julioefajardo | 0:7fd305c81a8e | 65 | pc.baud(115200); |
julioefajardo | 0:7fd305c81a8e | 66 | pc.printf("Hello World from FRDM-K64F board.\r\n"); |
julioefajardo | 0:7fd305c81a8e | 67 | kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
julioefajardo | 0:7fd305c81a8e | 68 | kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
julioefajardo | 0:7fd305c81a8e | 69 | kalman_init(&filter_pitch2, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
julioefajardo | 0:7fd305c81a8e | 70 | kalman_init(&filter_roll2, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
julioefajardo | 0:7fd305c81a8e | 71 | |
julioefajardo | 0:7fd305c81a8e | 72 | ProgramTimer.start(); |
julioefajardo | 0:7fd305c81a8e | 73 | loopStartTime = ProgramTimer.read_us(); |
julioefajardo | 0:7fd305c81a8e | 74 | timer = loopStartTime; |
julioefajardo | 0:7fd305c81a8e | 75 | |
julioefajardo | 0:7fd305c81a8e | 76 | while (true) { |
julioefajardo | 0:7fd305c81a8e | 77 | imu.readAcc(acc); |
julioefajardo | 0:7fd305c81a8e | 78 | AccRaw2G(acc); |
julioefajardo | 0:7fd305c81a8e | 79 | gyro.read(gyr); |
julioefajardo | 0:7fd305c81a8e | 80 | GyrRaw2D(gyr); |
julioefajardo | 0:7fd305c81a8e | 81 | |
julioefajardo | 0:7fd305c81a8e | 82 | imu2.readAcc(acc2); |
julioefajardo | 0:7fd305c81a8e | 83 | AccRaw2G2(acc2); |
julioefajardo | 0:7fd305c81a8e | 84 | gyro2.read(gyr2); |
julioefajardo | 0:7fd305c81a8e | 85 | GyrRaw2D2(gyr2); |
julioefajardo | 0:7fd305c81a8e | 86 | |
julioefajardo | 0:7fd305c81a8e | 87 | R = sqrt(std::pow(Acc.x, 2) + std::pow(Acc.y, 2) + std::pow(Acc.z, 2)); |
julioefajardo | 0:7fd305c81a8e | 88 | R2 = sqrt(std::pow(Acc2.x, 2) + std::pow(Acc2.y, 2) + std::pow(Acc2.z, 2)); |
julioefajardo | 0:7fd305c81a8e | 89 | |
julioefajardo | 0:7fd305c81a8e | 90 | kalman_predict(&filter_pitch, Gyr.x, (ProgramTimer.read_us() - timer)); |
julioefajardo | 0:7fd305c81a8e | 91 | kalman_update(&filter_pitch, acos(Acc.x/R)); |
julioefajardo | 0:7fd305c81a8e | 92 | kalman_predict(&filter_roll, Gyr.y, (ProgramTimer.read_us() - timer)); |
julioefajardo | 0:7fd305c81a8e | 93 | kalman_update(&filter_roll, acos(Acc.y/R)); |
julioefajardo | 0:7fd305c81a8e | 94 | |
julioefajardo | 0:7fd305c81a8e | 95 | kalman_predict(&filter_pitch2, Gyr2.x, (ProgramTimer.read_us() - timer)); |
julioefajardo | 0:7fd305c81a8e | 96 | kalman_update(&filter_pitch2, acos(Acc2.x/R2)); |
julioefajardo | 0:7fd305c81a8e | 97 | kalman_predict(&filter_roll2, Gyr.y, (ProgramTimer.read_us() - timer)); |
julioefajardo | 0:7fd305c81a8e | 98 | kalman_update(&filter_roll2, acos(Acc2.y/R2)); |
julioefajardo | 0:7fd305c81a8e | 99 | |
julioefajardo | 0:7fd305c81a8e | 100 | angle[0] = kalman_get_angle(&filter_pitch); |
julioefajardo | 0:7fd305c81a8e | 101 | angle[1] = kalman_get_angle(&filter_roll); |
julioefajardo | 0:7fd305c81a8e | 102 | |
julioefajardo | 0:7fd305c81a8e | 103 | angle2[0] = kalman_get_angle(&filter_pitch2); |
julioefajardo | 0:7fd305c81a8e | 104 | angle2[1] = kalman_get_angle(&filter_roll2); |
julioefajardo | 0:7fd305c81a8e | 105 | |
julioefajardo | 0:7fd305c81a8e | 106 | timer = ProgramTimer.read_us(); |
julioefajardo | 0:7fd305c81a8e | 107 | |
julioefajardo | 0:7fd305c81a8e | 108 | printf("IMU1\tangle0=%6.2f\tangle1=%6.2f\r\n",Rad2Dree*angle[0],Rad2Dree*angle[1]); |
julioefajardo | 0:7fd305c81a8e | 109 | printf("IMI2\tangle0=%6.2f\tangle1=%6.2f\r\n",Rad2Dree*angle2[0],Rad2Dree*angle2[1]); |
julioefajardo | 0:7fd305c81a8e | 110 | wait(0.2); |
julioefajardo | 0:7fd305c81a8e | 111 | } |
julioefajardo | 0:7fd305c81a8e | 112 | } |
julioefajardo | 0:7fd305c81a8e | 113 | |
julioefajardo | 0:7fd305c81a8e | 114 | void AccRaw2G(int acc[3]){ |
julioefajardo | 0:7fd305c81a8e | 115 | Acc.x = acc[0]/1024.0f; |
julioefajardo | 0:7fd305c81a8e | 116 | Acc.y = acc[1]/1024.0f; |
julioefajardo | 0:7fd305c81a8e | 117 | Acc.z = acc[2]/1024.0f; |
julioefajardo | 0:7fd305c81a8e | 118 | } |
julioefajardo | 0:7fd305c81a8e | 119 | |
julioefajardo | 0:7fd305c81a8e | 120 | void GyrRaw2D(short gyr[3]){ |
julioefajardo | 0:7fd305c81a8e | 121 | Gyr.x = gyr[0]/225.0f; |
julioefajardo | 0:7fd305c81a8e | 122 | Gyr.y = gyr[1]/225.0f; |
julioefajardo | 0:7fd305c81a8e | 123 | Gyr.z = gyr[2]/225.0f; |
julioefajardo | 0:7fd305c81a8e | 124 | } |
julioefajardo | 0:7fd305c81a8e | 125 | |
julioefajardo | 0:7fd305c81a8e | 126 | void AccRaw2G2(int acc[3]){ |
julioefajardo | 0:7fd305c81a8e | 127 | Acc2.x = acc[0]/1024.0f; |
julioefajardo | 0:7fd305c81a8e | 128 | Acc2.y = acc[1]/1024.0f; |
julioefajardo | 0:7fd305c81a8e | 129 | Acc2.z = acc[2]/1024.0f; |
julioefajardo | 0:7fd305c81a8e | 130 | } |
julioefajardo | 0:7fd305c81a8e | 131 | |
julioefajardo | 0:7fd305c81a8e | 132 | void GyrRaw2D2(short gyr[3]){ |
julioefajardo | 0:7fd305c81a8e | 133 | Gyr2.x = gyr[0]/225.0f; |
julioefajardo | 0:7fd305c81a8e | 134 | Gyr2.y = gyr[1]/225.0f; |
julioefajardo | 0:7fd305c81a8e | 135 | Gyr2.z = gyr[2]/225.0f; |
julioefajardo | 0:7fd305c81a8e | 136 | } |