d
Dependencies: X_NUCLEO_IKS01A1 kalman mbed
Fork of TEST-KalmanFilter by
Diff: main.cpp
- Revision:
- 1:39129aaf5c80
- Parent:
- 0:384bc42ad0cf
- Child:
- 2:cde1397d3fc4
- Child:
- 3:02877f5ca29e
diff -r 384bc42ad0cf -r 39129aaf5c80 main.cpp --- a/main.cpp Wed Aug 15 22:20:07 2012 +0000 +++ b/main.cpp Wed Mar 08 09:03:52 2017 +0000 @@ -1,14 +1,20 @@ #include "mbed.h" #include "kalman.c" -#include "BMA180.h" -#include "ITG3200.h" +#include "x_nucleo_iks01a1.h" +#include <math.h> #define PI 3.1415926535897932384626433832795 #define Rad2Dree 57.295779513082320876798154814105 -Serial pc(USBTX, USBRX); +/* Instantiate the expansion board */ +static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15); -I2C I2CBus(p28, p27); +/* Retrieve the composing elements of the expansion board */ +static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope(); +static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer(); +//static MagneticSensor *magnetometer = mems_expansion_board->magnetometer; + +Serial pc(USBTX, USBRX); Timer GlobalTime; Timer ProgramTimer; @@ -17,34 +23,18 @@ double angle[3]; unsigned long timer; long loopStartTime; +int32_t axesAcc[3]; +int32_t axesGyro[3]; -BMA180 Acc(I2CBus, GlobalTime); -ITG3200 Gyro(I2CBus, GlobalTime); -kalman filter_pitch; kalman filter_roll; +kalman filter_pitch; +kalman filter_yaw; int main() { - int count = 0; +// 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); +// GlobalTime.start(); // Parameters ( R_angle, Q_gyro, Q_angle ) kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); @@ -58,25 +48,28 @@ while(1) { //Aquire new values for the Gyro and Acc - Acc.Update(); - Gyro.Update(); + accelerometer->Get_X_Axes(axesAcc); + gyroscope->Get_G_Axes(axesGyro); + //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)); + R = sqrt(std::pow((float)axesAcc[0] , 2) + std::pow((float)axesAcc[1] , 2) + std::pow((float)axesAcc[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)); + kalman_predict(&filter_roll, axesGyro[0], (ProgramTimer.read_us() - timer)); + kalman_update(&filter_roll, acos(axesAcc[0]/R)); + kalman_predict(&filter_pitch, axesGyro[1], (ProgramTimer.read_us() - timer)); + kalman_update(&filter_pitch, acos(axesAcc[1]/R)); + kalman_predict(&filter_yaw, axesGyro[2], (ProgramTimer.read_us() - timer)); + kalman_update(&filter_yaw, acos(axesAcc[2]/R)); - angle[0] = kalman_get_angle(&filter_pitch); - angle[1] = kalman_get_angle(&filter_roll); + angle[0] = kalman_get_angle(&filter_roll); + angle[1] = kalman_get_angle(&filter_pitch); + angle[2] = kalman_get_angle(&filter_yaw); + 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]); - + pc.printf("Roll Angle X: %.6f Pitch Angle Y: %.6f Yaw Angle Z: %.6f \n\r", Rad2Dree * angle[0], Rad2Dree * angle[1], Rad2Dree * angle[2]); + } }