Program for testing the Kalman Filter. Receives input from gyro and accelerometer and oututs PITCH and ROLL.
Dependencies: mbed kalman ITG3200 BMA180
main.cpp@0:384bc42ad0cf, 2012-08-15 (annotated)
- Committer:
- cdonate
- Date:
- Wed Aug 15 22:20:07 2012 +0000
- Revision:
- 0:384bc42ad0cf
Program for testing the Kalman Filter
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cdonate | 0:384bc42ad0cf | 1 | #include "mbed.h" |
cdonate | 0:384bc42ad0cf | 2 | #include "kalman.c" |
cdonate | 0:384bc42ad0cf | 3 | #include "BMA180.h" |
cdonate | 0:384bc42ad0cf | 4 | #include "ITG3200.h" |
cdonate | 0:384bc42ad0cf | 5 | |
cdonate | 0:384bc42ad0cf | 6 | #define PI 3.1415926535897932384626433832795 |
cdonate | 0:384bc42ad0cf | 7 | #define Rad2Dree 57.295779513082320876798154814105 |
cdonate | 0:384bc42ad0cf | 8 | |
cdonate | 0:384bc42ad0cf | 9 | Serial pc(USBTX, USBRX); |
cdonate | 0:384bc42ad0cf | 10 | |
cdonate | 0:384bc42ad0cf | 11 | I2C I2CBus(p28, p27); |
cdonate | 0:384bc42ad0cf | 12 | |
cdonate | 0:384bc42ad0cf | 13 | Timer GlobalTime; |
cdonate | 0:384bc42ad0cf | 14 | Timer ProgramTimer; |
cdonate | 0:384bc42ad0cf | 15 | |
cdonate | 0:384bc42ad0cf | 16 | float R; |
cdonate | 0:384bc42ad0cf | 17 | double angle[3]; |
cdonate | 0:384bc42ad0cf | 18 | unsigned long timer; |
cdonate | 0:384bc42ad0cf | 19 | long loopStartTime; |
cdonate | 0:384bc42ad0cf | 20 | |
cdonate | 0:384bc42ad0cf | 21 | BMA180 Acc(I2CBus, GlobalTime); |
cdonate | 0:384bc42ad0cf | 22 | ITG3200 Gyro(I2CBus, GlobalTime); |
cdonate | 0:384bc42ad0cf | 23 | kalman filter_pitch; |
cdonate | 0:384bc42ad0cf | 24 | kalman filter_roll; |
cdonate | 0:384bc42ad0cf | 25 | |
cdonate | 0:384bc42ad0cf | 26 | int main() { |
cdonate | 0:384bc42ad0cf | 27 | int count = 0; |
cdonate | 0:384bc42ad0cf | 28 | pc.baud(115200); |
cdonate | 0:384bc42ad0cf | 29 | |
cdonate | 0:384bc42ad0cf | 30 | I2CBus.frequency(400000); |
cdonate | 0:384bc42ad0cf | 31 | GlobalTime.start(); |
cdonate | 0:384bc42ad0cf | 32 | |
cdonate | 0:384bc42ad0cf | 33 | Acc.Init(); |
cdonate | 0:384bc42ad0cf | 34 | wait_ms(500); |
cdonate | 0:384bc42ad0cf | 35 | |
cdonate | 0:384bc42ad0cf | 36 | //User Calibration |
cdonate | 0:384bc42ad0cf | 37 | short Raw1g[3]= {0, 0, 0}; |
cdonate | 0:384bc42ad0cf | 38 | Acc.userCalibration(Raw1g); |
cdonate | 0:384bc42ad0cf | 39 | |
cdonate | 0:384bc42ad0cf | 40 | //0.5s Calibration |
cdonate | 0:384bc42ad0cf | 41 | Acc.Calibrate(500, Raw1g); |
cdonate | 0:384bc42ad0cf | 42 | |
cdonate | 0:384bc42ad0cf | 43 | Gyro.Init(); |
cdonate | 0:384bc42ad0cf | 44 | wait_ms(500); |
cdonate | 0:384bc42ad0cf | 45 | |
cdonate | 0:384bc42ad0cf | 46 | //0.5s Calibration |
cdonate | 0:384bc42ad0cf | 47 | Gyro.Calibrate(500); |
cdonate | 0:384bc42ad0cf | 48 | |
cdonate | 0:384bc42ad0cf | 49 | // Parameters ( R_angle, Q_gyro, Q_angle ) |
cdonate | 0:384bc42ad0cf | 50 | kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
cdonate | 0:384bc42ad0cf | 51 | kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
cdonate | 0:384bc42ad0cf | 52 | |
cdonate | 0:384bc42ad0cf | 53 | |
cdonate | 0:384bc42ad0cf | 54 | ProgramTimer.start(); |
cdonate | 0:384bc42ad0cf | 55 | loopStartTime = ProgramTimer.read_us(); |
cdonate | 0:384bc42ad0cf | 56 | timer = loopStartTime; |
cdonate | 0:384bc42ad0cf | 57 | |
cdonate | 0:384bc42ad0cf | 58 | while(1) { |
cdonate | 0:384bc42ad0cf | 59 | |
cdonate | 0:384bc42ad0cf | 60 | //Aquire new values for the Gyro and Acc |
cdonate | 0:384bc42ad0cf | 61 | Acc.Update(); |
cdonate | 0:384bc42ad0cf | 62 | Gyro.Update(); |
cdonate | 0:384bc42ad0cf | 63 | |
cdonate | 0:384bc42ad0cf | 64 | //Calcuate the resulting vector R from the 3 acc axes |
cdonate | 0:384bc42ad0cf | 65 | R = sqrt(std::pow(Acc.Acc[0] , 2) + std::pow(Acc.Acc[1] , 2) + std::pow(Acc.Acc[2] , 2)); |
cdonate | 0:384bc42ad0cf | 66 | |
cdonate | 0:384bc42ad0cf | 67 | kalman_predict(&filter_pitch, Gyro.Rate[0], (ProgramTimer.read_us() - timer)); |
cdonate | 0:384bc42ad0cf | 68 | kalman_update(&filter_pitch, acos(Acc.Acc[0]/R)); |
cdonate | 0:384bc42ad0cf | 69 | kalman_predict(&filter_roll, Gyro.Rate[1], (ProgramTimer.read_us() - timer)); |
cdonate | 0:384bc42ad0cf | 70 | kalman_update(&filter_roll, acos(Acc.Acc[1]/R)); |
cdonate | 0:384bc42ad0cf | 71 | |
cdonate | 0:384bc42ad0cf | 72 | angle[0] = kalman_get_angle(&filter_pitch); |
cdonate | 0:384bc42ad0cf | 73 | angle[1] = kalman_get_angle(&filter_roll); |
cdonate | 0:384bc42ad0cf | 74 | |
cdonate | 0:384bc42ad0cf | 75 | timer = ProgramTimer.read_us(); |
cdonate | 0:384bc42ad0cf | 76 | |
cdonate | 0:384bc42ad0cf | 77 | //count++; |
cdonate | 0:384bc42ad0cf | 78 | //if(count == 10000){pc.printf("Program time: %i\n\r", ProgramTimer.read_us() - loopStartTime);loopStartTime = ProgramTimer.read_us(); count = 0;} |
cdonate | 0:384bc42ad0cf | 79 | pc.printf("Pitch Angle X: %.6f Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]); |
cdonate | 0:384bc42ad0cf | 80 | |
cdonate | 0:384bc42ad0cf | 81 | } |
cdonate | 0:384bc42ad0cf | 82 | } |