ST_DuckieTeam / Mbed 2 deprecated TEST-KalmanFilter

Versione funzionante che utilizza giroscopio ed accelerometro per calcolare roll e pitch

Dependencies:   X_NUCLEO_IKS01A1 kalman mbed

Fork of KalmanFilter by Claudio Donate

main.cpp@2:cde1397d3fc4, 2017-03-08 (annotated)

Committer:
Carminio
Date:
Wed Mar 08 10:11:54 2017 +0000
Revision:
2:cde1397d3fc4
Parent:
1:39129aaf5c80 
Ver. OK con calcolo roll e pitch. Bisogna solo risolvere problema singolarita'.

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 2:cde1397d3fc4 12
Carminio 1:39129aaf5c80 13 /* Retrieve the composing elements of the expansion board */
Carminio 1:39129aaf5c80 14 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
Carminio 1:39129aaf5c80 15 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
Carminio 1:39129aaf5c80 16 //static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
Carminio 1:39129aaf5c80 17
Carminio 1:39129aaf5c80 18 Serial pc(USBTX, USBRX);
cdonate 0:384bc42ad0cf 19
cdonate 0:384bc42ad0cf 20 Timer GlobalTime;
cdonate 0:384bc42ad0cf 21 Timer ProgramTimer;
cdonate 0:384bc42ad0cf 22
cdonate 0:384bc42ad0cf 23 float R;
cdonate 0:384bc42ad0cf 24 double angle[3];
cdonate 0:384bc42ad0cf 25 unsigned long timer;
cdonate 0:384bc42ad0cf 26 long loopStartTime;
Carminio 1:39129aaf5c80 27 int32_t axesAcc[3];
Carminio 1:39129aaf5c80 28 int32_t axesGyro[3];
cdonate 0:384bc42ad0cf 29
cdonate 0:384bc42ad0cf 30 kalman filter_roll;
Carminio 1:39129aaf5c80 31 kalman filter_pitch;
Carminio 1:39129aaf5c80 32 kalman filter_yaw;
cdonate 0:384bc42ad0cf 33
cdonate 0:384bc42ad0cf 34 int main() {
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 2:cde1397d3fc4 62
cdonate 0:384bc42ad0cf 63
Carminio 2:cde1397d3fc4 64 angle[0] = (kalman_get_angle(&filter_roll)*Rad2Dree)-90;
Carminio 2:cde1397d3fc4 65 angle[1] = -(kalman_get_angle(&filter_pitch)*Rad2Dree)+90;
Carminio 2:cde1397d3fc4 66
cdonate 0:384bc42ad0cf 67 timer = ProgramTimer.read_us();
cdonate 0:384bc42ad0cf 68
Carminio 2:cde1397d3fc4 69 pc.printf("Roll Angle X: %.6f Pitch Angle Y: %.6f \n\r", angle[0], angle[1]);
Carminio 1:39129aaf5c80 70
cdonate 0:384bc42ad0cf 71 }
cdonate 0:384bc42ad0cf 72 }