d
Dependencies: X_NUCLEO_IKS01A1 kalman mbed
Fork of TEST-KalmanFilter by
main.cpp
- Committer:
- rendek4
- Date:
- 2017-05-22
- Revision:
- 4:7c9469bb52f4
- Parent:
- 3:02877f5ca29e
File content as of revision 4:7c9469bb52f4:
#include "mbed.h" #include "kalman.c" #include "x_nucleo_iks01a1.h" #include <math.h> #define PI 3.1415926535897932384626433832795 #define Rad2Dree 57.295779513082320876798154814105 /* Instantiate the expansion board */ static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15); /* 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; float R; double angle[3]; unsigned long timer; long loopStartTime; int32_t axesAcc[3]; int32_t axesGyro[3]; int32_t axesMag[3]; kalman filter_roll; kalman filter_pitch; kalman filter_yaw; int main() { pc.baud(9600); // GlobalTime.start(); // 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); kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); ProgramTimer.start(); loopStartTime = ProgramTimer.read_us(); timer = loopStartTime; while(1) { // wait_ms(10); //Aquire new values for the Gyro and Acc accelerometer->Get_X_Axes(axesAcc); gyroscope->Get_G_Axes(axesGyro); magnetometer->Get_M_Axes(axesMag); //Calculate the resulting vector R from the 3 acc axes R = sqrt(std::pow((float)axesAcc[0] , 2) + std::pow((float)axesAcc[1] , 2) + std::pow((float)axesAcc[2] , 2)); 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(axesMag[2]/R)); angle[0] = kalman_get_angle(&filter_roll); angle[1] = kalman_get_angle(&filter_pitch); //angle[2] = atan2((double)axesGyro[0],(double)axesGyro[1])*Rad2Dree; angle[2] = kalman_get_angle(&filter_yaw); atan2((double)axesAcc[0],(double)axesAcc[1])*Rad2Dree; timer = ProgramTimer.read_us(); pc.printf("%.6f ;\t %.6f;\t %.6f; \n\r", Rad2Dree * angle[0], Rad2Dree * angle[1], Rad2Dree * angle[2]); } }