Patrik Rendek / Mbed 2 deprecated CSS2-KalmanFilter

d

Dependencies:   X_NUCLEO_IKS01A1 kalman mbed

Fork of TEST-KalmanFilter by ST_DuckieTeam

main.cpp@1:39129aaf5c80, 2017-03-08 (annotated)

Committer:
Carminio
Date:
Wed Mar 08 09:03:52 2017 +0000
Revision:
1:39129aaf5c80
Parent:
0:384bc42ad0cf
Child:
2:cde1397d3fc4
Child:
3:02877f5ca29e
Test ver. non funzionante con aggiunta yaw

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 1:39129aaf5c80 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];
cdonate 0:384bc42ad0cf 28
cdonate 0:384bc42ad0cf 29 kalman filter_roll;
Carminio 1:39129aaf5c80 30 kalman filter_pitch;
Carminio 1:39129aaf5c80 31 kalman filter_yaw;
cdonate 0:384bc42ad0cf 32
cdonate 0:384bc42ad0cf 33 int main() {
Carminio 1:39129aaf5c80 34 // int count = 0;
cdonate 0:384bc42ad0cf 35 pc.baud(115200);
cdonate 0:384bc42ad0cf 36
Carminio 1:39129aaf5c80 37 // GlobalTime.start();
cdonate 0:384bc42ad0cf 38
cdonate 0:384bc42ad0cf 39 // Parameters ( R_angle, Q_gyro, Q_angle )
cdonate 0:384bc42ad0cf 40 kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
cdonate 0:384bc42ad0cf 41 kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
cdonate 0:384bc42ad0cf 42
cdonate 0:384bc42ad0cf 43
cdonate 0:384bc42ad0cf 44 ProgramTimer.start();
cdonate 0:384bc42ad0cf 45 loopStartTime = ProgramTimer.read_us();
cdonate 0:384bc42ad0cf 46 timer = loopStartTime;
cdonate 0:384bc42ad0cf 47
cdonate 0:384bc42ad0cf 48 while(1) {
cdonate 0:384bc42ad0cf 49
cdonate 0:384bc42ad0cf 50 //Aquire new values for the Gyro and Acc
Carminio 1:39129aaf5c80 51 accelerometer->Get_X_Axes(axesAcc);
Carminio 1:39129aaf5c80 52 gyroscope->Get_G_Axes(axesGyro);
Carminio 1:39129aaf5c80 53
cdonate 0:384bc42ad0cf 54
cdonate 0:384bc42ad0cf 55 //Calcuate the resulting vector R from the 3 acc axes
Carminio 1:39129aaf5c80 56 R = sqrt(std::pow((float)axesAcc[0] , 2) + std::pow((float)axesAcc[1] , 2) + std::pow((float)axesAcc[2] , 2));
cdonate 0:384bc42ad0cf 57
Carminio 1:39129aaf5c80 58 kalman_predict(&filter_roll, axesGyro[0], (ProgramTimer.read_us() - timer));
Carminio 1:39129aaf5c80 59 kalman_update(&filter_roll, acos(axesAcc[0]/R));
Carminio 1:39129aaf5c80 60 kalman_predict(&filter_pitch, axesGyro[1], (ProgramTimer.read_us() - timer));
Carminio 1:39129aaf5c80 61 kalman_update(&filter_pitch, acos(axesAcc[1]/R));
Carminio 1:39129aaf5c80 62 kalman_predict(&filter_yaw, axesGyro[2], (ProgramTimer.read_us() - timer));
Carminio 1:39129aaf5c80 63 kalman_update(&filter_yaw, acos(axesAcc[2]/R));
cdonate 0:384bc42ad0cf 64
cdonate 0:384bc42ad0cf 65
Carminio 1:39129aaf5c80 66 angle[0] = kalman_get_angle(&filter_roll);
Carminio 1:39129aaf5c80 67 angle[1] = kalman_get_angle(&filter_pitch);
Carminio 1:39129aaf5c80 68 angle[2] = kalman_get_angle(&filter_yaw);
Carminio 1:39129aaf5c80 69
cdonate 0:384bc42ad0cf 70 timer = ProgramTimer.read_us();
cdonate 0:384bc42ad0cf 71
Carminio 1:39129aaf5c80 72 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]);
Carminio 1:39129aaf5c80 73
cdonate 0:384bc42ad0cf 74 }
cdonate 0:384bc42ad0cf 75 }