Versione funzionante che utilizza giroscopio ed accelerometro per calcolare roll e pitch
Dependencies: X_NUCLEO_IKS01A1 kalman mbed
Fork of KalmanFilter by
Diff: main.cpp
- Revision:
- 2:cde1397d3fc4
- Parent:
- 1:39129aaf5c80
diff -r 39129aaf5c80 -r cde1397d3fc4 main.cpp --- a/main.cpp Wed Mar 08 09:03:52 2017 +0000 +++ b/main.cpp Wed Mar 08 10:11:54 2017 +0000 @@ -9,6 +9,7 @@ /* 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(); @@ -31,7 +32,6 @@ kalman filter_yaw; int main() { -// int count = 0; pc.baud(115200); // GlobalTime.start(); @@ -59,17 +59,14 @@ 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)); - + - angle[0] = kalman_get_angle(&filter_roll); - angle[1] = kalman_get_angle(&filter_pitch); - angle[2] = kalman_get_angle(&filter_yaw); - + angle[0] = (kalman_get_angle(&filter_roll)*Rad2Dree)-90; + angle[1] = -(kalman_get_angle(&filter_pitch)*Rad2Dree)+90; + 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]); + pc.printf("Roll Angle X: %.6f Pitch Angle Y: %.6f \n\r", angle[0], angle[1]); } }