Versione funzionante che utilizza giroscopio ed accelerometro per calcolare roll e pitch
Dependencies: X_NUCLEO_IKS01A1 kalman mbed
Fork of KalmanFilter by
Revision 3:02877f5ca29e, committed 2017-03-09
- Comitter:
- Carminio
- Date:
- Thu Mar 09 13:14:31 2017 +0000
- Parent:
- 1:39129aaf5c80
- Commit message:
- TestVer for yaw calculation. Not working.
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 39129aaf5c80 -r 02877f5ca29e main.cpp --- a/main.cpp Wed Mar 08 09:03:52 2017 +0000 +++ b/main.cpp Thu Mar 09 13:14:31 2017 +0000 @@ -12,7 +12,7 @@ /* 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; +static MagneticSensor *magnetometer = mems_expansion_board->magnetometer; Serial pc(USBTX, USBRX); @@ -25,13 +25,13 @@ 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() { -// int count = 0; pc.baud(115200); // GlobalTime.start(); @@ -40,32 +40,38 @@ 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); - //Calcuate the resulting vector R from the 3 acc axes + //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(axesAcc[2]/R)); + kalman_update(&filter_yaw, acos(axesMag[2]/R)); angle[0] = kalman_get_angle(&filter_roll); angle[1] = kalman_get_angle(&filter_pitch); - angle[2] = kalman_get_angle(&filter_yaw); + 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();