Versione funzionante che utilizza giroscopio ed accelerometro per calcolare roll e pitch
Dependencies: X_NUCLEO_IKS01A1 kalman mbed
Fork of KalmanFilter by
Diff: main.cpp
- Revision:
- 0:384bc42ad0cf
- Child:
- 1:39129aaf5c80
diff -r 000000000000 -r 384bc42ad0cf main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Aug 15 22:20:07 2012 +0000 @@ -0,0 +1,82 @@ +#include "mbed.h" +#include "kalman.c" +#include "BMA180.h" +#include "ITG3200.h" + +#define PI 3.1415926535897932384626433832795 +#define Rad2Dree 57.295779513082320876798154814105 + +Serial pc(USBTX, USBRX); + +I2C I2CBus(p28, p27); + +Timer GlobalTime; +Timer ProgramTimer; + +float R; +double angle[3]; +unsigned long timer; +long loopStartTime; + +BMA180 Acc(I2CBus, GlobalTime); +ITG3200 Gyro(I2CBus, GlobalTime); +kalman filter_pitch; +kalman filter_roll; + +int main() { + int count = 0; + pc.baud(115200); + + I2CBus.frequency(400000); + GlobalTime.start(); + + Acc.Init(); + wait_ms(500); + + //User Calibration + short Raw1g[3]= {0, 0, 0}; + Acc.userCalibration(Raw1g); + + //0.5s Calibration + Acc.Calibrate(500, Raw1g); + + Gyro.Init(); + wait_ms(500); + + //0.5s Calibration + Gyro.Calibrate(500); + + // Parameters ( R_angle, Q_gyro, Q_angle ) + kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); + kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); + + + ProgramTimer.start(); + loopStartTime = ProgramTimer.read_us(); + timer = loopStartTime; + + while(1) { + + //Aquire new values for the Gyro and Acc + Acc.Update(); + Gyro.Update(); + + //Calcuate the resulting vector R from the 3 acc axes + R = sqrt(std::pow(Acc.Acc[0] , 2) + std::pow(Acc.Acc[1] , 2) + std::pow(Acc.Acc[2] , 2)); + + kalman_predict(&filter_pitch, Gyro.Rate[0], (ProgramTimer.read_us() - timer)); + kalman_update(&filter_pitch, acos(Acc.Acc[0]/R)); + kalman_predict(&filter_roll, Gyro.Rate[1], (ProgramTimer.read_us() - timer)); + kalman_update(&filter_roll, acos(Acc.Acc[1]/R)); + + angle[0] = kalman_get_angle(&filter_pitch); + angle[1] = kalman_get_angle(&filter_roll); + + timer = ProgramTimer.read_us(); + + //count++; + //if(count == 10000){pc.printf("Program time: %i\n\r", ProgramTimer.read_us() - loopStartTime);loopStartTime = ProgramTimer.read_us(); count = 0;} + pc.printf("Pitch Angle X: %.6f Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]); + + } +}