Patrik Rendek / Mbed 2 deprecated CSS2-KalmanFilter

d

Dependencies:   X_NUCLEO_IKS01A1 kalman mbed

Fork of TEST-KalmanFilter by ST_DuckieTeam

main.cpp@4:7c9469bb52f4, 2017-05-22 (annotated)

Committer:
rendek4
Date:
Mon May 22 18:45:18 2017 +0000
Revision:
4:7c9469bb52f4
Parent:
3:02877f5ca29e 
aaa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cdonate 0:384bc42ad0cf 1 #include "mbed.h"
cdonate 0:384bc42ad0cf 2 #include "kalman.c"
Carminio 1:39129aaf5c80 3 #include "x_nucleo_iks01a1.h"
Carminio 1:39129aaf5c80 4 #include <math.h>
cdonate 0:384bc42ad0cf 5
cdonate 0:384bc42ad0cf 6 #define PI 3.1415926535897932384626433832795
cdonate 0:384bc42ad0cf 7 #define Rad2Dree 57.295779513082320876798154814105
cdonate 0:384bc42ad0cf 8
Carminio 1:39129aaf5c80 9 /* Instantiate the expansion board */
Carminio 1:39129aaf5c80 10 static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
cdonate 0:384bc42ad0cf 11
Carminio 1:39129aaf5c80 12 /* Retrieve the composing elements of the expansion board */
Carminio 1:39129aaf5c80 13 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
Carminio 1:39129aaf5c80 14 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
Carminio 3:02877f5ca29e 15 static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
Carminio 1:39129aaf5c80 16
Carminio 1:39129aaf5c80 17 Serial pc(USBTX, USBRX);
cdonate 0:384bc42ad0cf 18
cdonate 0:384bc42ad0cf 19 Timer GlobalTime;
cdonate 0:384bc42ad0cf 20 Timer ProgramTimer;
cdonate 0:384bc42ad0cf 21
cdonate 0:384bc42ad0cf 22 float R;
cdonate 0:384bc42ad0cf 23 double angle[3];
cdonate 0:384bc42ad0cf 24 unsigned long timer;
cdonate 0:384bc42ad0cf 25 long loopStartTime;
Carminio 1:39129aaf5c80 26 int32_t axesAcc[3];
Carminio 1:39129aaf5c80 27 int32_t axesGyro[3];
Carminio 3:02877f5ca29e 28 int32_t axesMag[3];
cdonate 0:384bc42ad0cf 29
cdonate 0:384bc42ad0cf 30 kalman filter_roll;
Carminio 1:39129aaf5c80 31 kalman filter_pitch;
rendek4 4:7c9469bb52f4 32 kalman filter_yaw;
rendek4 4:7c9469bb52f4 33
rendek4 4:7c9469bb52f4 34 int main()
rendek4 4:7c9469bb52f4 35 {
rendek4 4:7c9469bb52f4 36 pc.baud(9600);
rendek4 4:7c9469bb52f4 37
Carminio 1:39129aaf5c80 38 // GlobalTime.start();
cdonate 0:384bc42ad0cf 39
rendek4 4:7c9469bb52f4 40 // Parameters ( R_angle, Q_gyro, Q_angle )
rendek4 4:7c9469bb52f4 41 kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
rendek4 4:7c9469bb52f4 42 kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
rendek4 4:7c9469bb52f4 43
Carminio 3:02877f5ca29e 44 kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
rendek4 4:7c9469bb52f4 45
rendek4 4:7c9469bb52f4 46
cdonate 0:384bc42ad0cf 47 ProgramTimer.start();
cdonate 0:384bc42ad0cf 48 loopStartTime = ProgramTimer.read_us();
cdonate 0:384bc42ad0cf 49 timer = loopStartTime;
rendek4 4:7c9469bb52f4 50
cdonate 0:384bc42ad0cf 51 while(1) {
Carminio 3:02877f5ca29e 52 // wait_ms(10);
cdonate 0:384bc42ad0cf 53 //Aquire new values for the Gyro and Acc
rendek4 4:7c9469bb52f4 54 accelerometer->Get_X_Axes(axesAcc);
rendek4 4:7c9469bb52f4 55 gyroscope->Get_G_Axes(axesGyro);
rendek4 4:7c9469bb52f4 56 magnetometer->Get_M_Axes(axesMag);
rendek4 4:7c9469bb52f4 57
Carminio 3:02877f5ca29e 58 //Calculate the resulting vector R from the 3 acc axes
Carminio 1:39129aaf5c80 59 R = sqrt(std::pow((float)axesAcc[0] , 2) + std::pow((float)axesAcc[1] , 2) + std::pow((float)axesAcc[2] , 2));
rendek4 4:7c9469bb52f4 60
rendek4 4:7c9469bb52f4 61 kalman_predict(&filter_roll, axesGyro[0], (ProgramTimer.read_us() - timer));
Carminio 1:39129aaf5c80 62 kalman_update(&filter_roll, acos(axesAcc[0]/R));
rendek4 4:7c9469bb52f4 63
rendek4 4:7c9469bb52f4 64 kalman_predict(&filter_pitch, axesGyro[1], (ProgramTimer.read_us() - timer));
Carminio 1:39129aaf5c80 65 kalman_update(&filter_pitch, acos(axesAcc[1]/R));
rendek4 4:7c9469bb52f4 66
rendek4 4:7c9469bb52f4 67 kalman_predict(&filter_yaw, axesGyro[2], (ProgramTimer.read_us() - timer));
Carminio 3:02877f5ca29e 68 kalman_update(&filter_yaw, acos(axesMag[2]/R));
rendek4 4:7c9469bb52f4 69
rendek4 4:7c9469bb52f4 70
Carminio 1:39129aaf5c80 71 angle[0] = kalman_get_angle(&filter_roll);
Carminio 1:39129aaf5c80 72 angle[1] = kalman_get_angle(&filter_pitch);
rendek4 4:7c9469bb52f4 73 //angle[2] = atan2((double)axesGyro[0],(double)axesGyro[1])*Rad2Dree;
rendek4 4:7c9469bb52f4 74 angle[2] = kalman_get_angle(&filter_yaw);
rendek4 4:7c9469bb52f4 75 atan2((double)axesAcc[0],(double)axesAcc[1])*Rad2Dree;
rendek4 4:7c9469bb52f4 76
cdonate 0:384bc42ad0cf 77 timer = ProgramTimer.read_us();
rendek4 4:7c9469bb52f4 78
rendek4 4:7c9469bb52f4 79
rendek4 4:7c9469bb52f4 80 pc.printf("%.6f ;\t %.6f;\t %.6f; \n\r", Rad2Dree * angle[0], Rad2Dree * angle[1], Rad2Dree * angle[2]);
rendek4 4:7c9469bb52f4 81
cdonate 0:384bc42ad0cf 82 }
cdonate 0:384bc42ad0cf 83 }