Julio Fajardo / Mbed 2 deprecated Galileo_HoverBoardRider

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?

UserRevisionLine numberNew 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 }