Versione funzionante che utilizza giroscopio ed accelerometro per calcolare roll e pitch
Dependencies: X_NUCLEO_IKS01A1 kalman mbed
Fork of KalmanFilter by
main.cpp
- Committer:
- cdonate
- Date:
- 2012-08-15
- Revision:
- 0:384bc42ad0cf
- Child:
- 1:39129aaf5c80
File content as of revision 0:384bc42ad0cf:
#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]); } }