Versione funzionante che utilizza giroscopio ed accelerometro per calcolare roll e pitch
Dependencies: X_NUCLEO_IKS01A1 kalman mbed
Fork of KalmanFilter by
main.cpp
- Committer:
- Carminio
- Date:
- 2017-03-09
- Revision:
- 3:02877f5ca29e
- Parent:
- 1:39129aaf5c80
File content as of revision 3:02877f5ca29e:
#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]; int32_t axesMag[3]; kalman filter_roll; kalman filter_pitch; kalman filter_yaw; int main() { 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); kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); ProgramTimer.start(); loopStartTime = ProgramTimer.read_us(); timer = loopStartTime; while(1) { // wait_ms(10); //Aquire new values for the Gyro and Acc accelerometer->Get_X_Axes(axesAcc); gyroscope->Get_G_Axes(axesGyro); magnetometer->Get_M_Axes(axesMag); //Calculate 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(axesMag[2]/R)); angle[0] = kalman_get_angle(&filter_roll); angle[1] = kalman_get_angle(&filter_pitch); angle[2] = atan2((double)axesGyro[0],(double)axesGyro[1])*Rad2Dree; // angle[2] = kalman_get_angle(&filter_yaw); atan2((double)axesAcc[0],(double)axesAcc[1])*Rad2Dree; 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]); } }