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@3:02877f5ca29e, 2017-03-09 (annotated)

Committer:
Carminio
Date:
Thu Mar 09 13:14:31 2017 +0000
Revision:
3:02877f5ca29e
Parent:
1:39129aaf5c80 
TestVer for yaw calculation. Not working.

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;
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
Carminio 3:02877f5ca29e 43 kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
Carminio 3:02877f5ca29e 44
cdonate 0:384bc42ad0cf 45
cdonate 0:384bc42ad0cf 46 ProgramTimer.start();
cdonate 0:384bc42ad0cf 47 loopStartTime = ProgramTimer.read_us();
cdonate 0:384bc42ad0cf 48 timer = loopStartTime;
cdonate 0:384bc42ad0cf 49
cdonate 0:384bc42ad0cf 50 while(1) {
Carminio 3:02877f5ca29e 51 // wait_ms(10);
cdonate 0:384bc42ad0cf 52 //Aquire new values for the Gyro and Acc
Carminio 1:39129aaf5c80 53 accelerometer->Get_X_Axes(axesAcc);
Carminio 1:39129aaf5c80 54 gyroscope->Get_G_Axes(axesGyro);
Carminio 3:02877f5ca29e 55 magnetometer->Get_M_Axes(axesMag);
cdonate 0:384bc42ad0cf 56
Carminio 3:02877f5ca29e 57 //Calculate the resulting vector R from the 3 acc axes
Carminio 1:39129aaf5c80 58 R = sqrt(std::pow((float)axesAcc[0] , 2) + std::pow((float)axesAcc[1] , 2) + std::pow((float)axesAcc[2] , 2));
cdonate 0:384bc42ad0cf 59
Carminio 1:39129aaf5c80 60 kalman_predict(&filter_roll, axesGyro[0], (ProgramTimer.read_us() - timer));
Carminio 1:39129aaf5c80 61 kalman_update(&filter_roll, acos(axesAcc[0]/R));
Carminio 3:02877f5ca29e 62
Carminio 1:39129aaf5c80 63 kalman_predict(&filter_pitch, axesGyro[1], (ProgramTimer.read_us() - timer));
Carminio 1:39129aaf5c80 64 kalman_update(&filter_pitch, acos(axesAcc[1]/R));
Carminio 3:02877f5ca29e 65
Carminio 1:39129aaf5c80 66 kalman_predict(&filter_yaw, axesGyro[2], (ProgramTimer.read_us() - timer));
Carminio 3:02877f5ca29e 67 kalman_update(&filter_yaw, acos(axesMag[2]/R));
cdonate 0:384bc42ad0cf 68
cdonate 0:384bc42ad0cf 69
Carminio 1:39129aaf5c80 70 angle[0] = kalman_get_angle(&filter_roll);
Carminio 1:39129aaf5c80 71 angle[1] = kalman_get_angle(&filter_pitch);
Carminio 3:02877f5ca29e 72 angle[2] = atan2((double)axesGyro[0],(double)axesGyro[1])*Rad2Dree;
Carminio 3:02877f5ca29e 73 // angle[2] = kalman_get_angle(&filter_yaw);
Carminio 3:02877f5ca29e 74 atan2((double)axesAcc[0],(double)axesAcc[1])*Rad2Dree;
Carminio 1:39129aaf5c80 75
cdonate 0:384bc42ad0cf 76 timer = ProgramTimer.read_us();
cdonate 0:384bc42ad0cf 77
Carminio 1:39129aaf5c80 78 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 79
cdonate 0:384bc42ad0cf 80 }
cdonate 0:384bc42ad0cf 81 }