d
Dependencies: X_NUCLEO_IKS01A1 kalman mbed
Fork of TEST-KalmanFilter by
main.cpp
- Committer:
- Carminio
- Date:
- 2017-03-08
- Revision:
- 1:39129aaf5c80
- Parent:
- 0:384bc42ad0cf
- Child:
- 2:cde1397d3fc4
- Child:
- 3:02877f5ca29e
File content as of revision 1:39129aaf5c80:
#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]; kalman filter_roll; kalman filter_pitch; kalman filter_yaw; int main() { // int count = 0; pc.baud(115200); // 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); ProgramTimer.start(); loopStartTime = ProgramTimer.read_us(); timer = loopStartTime; while(1) { //Aquire new values for the Gyro and Acc accelerometer->Get_X_Axes(axesAcc); gyroscope->Get_G_Axes(axesGyro); //Calcuate 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(axesAcc[2]/R)); 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(); 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]); } }