Patrik Rendek / Mbed 2 deprecated CSS2-KalmanFilter

d

Dependencies:   X_NUCLEO_IKS01A1 kalman mbed

Fork of TEST-KalmanFilter by ST_DuckieTeam

main.cpp@0:384bc42ad0cf, 2012-08-15 (annotated)

Committer:
cdonate
Date:
Wed Aug 15 22:20:07 2012 +0000
Revision:
0:384bc42ad0cf
Child:
1:39129aaf5c80
Program for testing the Kalman Filter

Who changed what in which revision?

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