Yuri De Stefani
controlador de atitude
AttitudeEstimator/AttitudeEstimator.h@8:c96125e9ac70, 2018-11-21 (annotated)
- Committer:
- yurindes
- Date:
- Wed Nov 21 10:07:00 2018 +0000
- Branch:
- yuri
- Revision:
- 8:c96125e9ac70
- Parent:
- 1:579511e9f0b8
teste controlador atitude;
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| yvesyuzo | 1:579511e9f0b8 | 1 | #ifndef AttitudeEstimator_h |
| yvesyuzo | 1:579511e9f0b8 | 2 | #define AttitudeEstimator_h |
| yvesyuzo | 1:579511e9f0b8 | 3 | |
| yvesyuzo | 1:579511e9f0b8 | 4 | #include "mbed.h" |
| yvesyuzo | 1:579511e9f0b8 | 5 | #include "MPU9250.h" |
| yvesyuzo | 1:579511e9f0b8 | 6 | #include "Library.h" |
| yvesyuzo | 1:579511e9f0b8 | 7 | |
| yvesyuzo | 1:579511e9f0b8 | 8 | /* |
| yvesyuzo | 1:579511e9f0b8 | 9 | // Estimator constants |
| yvesyuzo | 1:579511e9f0b8 | 10 | float const pi = 3.14159265f; |
| yvesyuzo | 1:579511e9f0b8 | 11 | float const dt = 0.005f; |
| yvesyuzo | 1:579511e9f0b8 | 12 | float const rho = 0.05f; |
| yvesyuzo | 1:579511e9f0b8 | 13 | */ |
| yvesyuzo | 1:579511e9f0b8 | 14 | |
| yvesyuzo | 1:579511e9f0b8 | 15 | |
| yvesyuzo | 1:579511e9f0b8 | 16 | // Attitude estimator class |
| yvesyuzo | 1:579511e9f0b8 | 17 | class AttitudeEstimator |
| yvesyuzo | 1:579511e9f0b8 | 18 | { |
| yvesyuzo | 1:579511e9f0b8 | 19 | public : |
| yvesyuzo | 1:579511e9f0b8 | 20 | // Class constructor |
| yvesyuzo | 1:579511e9f0b8 | 21 | AttitudeEstimator () ; |
| yvesyuzo | 1:579511e9f0b8 | 22 | // Initialize class |
| yvesyuzo | 1:579511e9f0b8 | 23 | void init () ; |
| yvesyuzo | 1:579511e9f0b8 | 24 | // Estimate Euler angles (rad ) and angular velocities ( rad /s) |
| yvesyuzo | 1:579511e9f0b8 | 25 | void estimate () ; |
| yvesyuzo | 1:579511e9f0b8 | 26 | // Euler angles ( rad) |
| yvesyuzo | 1:579511e9f0b8 | 27 | float phi , theta , psi ; |
| yvesyuzo | 1:579511e9f0b8 | 28 | // Angular velocities ( rad /s) |
| yvesyuzo | 1:579511e9f0b8 | 29 | float p, q, r; |
| yvesyuzo | 1:579511e9f0b8 | 30 | |
| yvesyuzo | 1:579511e9f0b8 | 31 | private : |
| yvesyuzo | 1:579511e9f0b8 | 32 | // IMU sensor object |
| yvesyuzo | 1:579511e9f0b8 | 33 | MPU9250 imu; |
| yvesyuzo | 1:579511e9f0b8 | 34 | // Calibrates gyroscope by calculating angular velocity bias (rad/s) |
| yvesyuzo | 1:579511e9f0b8 | 35 | void calibrate_gyro () ; |
| yvesyuzo | 1:579511e9f0b8 | 36 | // Estimate Euler angles (rad ) from accelerometer data |
| yvesyuzo | 1:579511e9f0b8 | 37 | void estimate_accel () ; |
| yvesyuzo | 1:579511e9f0b8 | 38 | // Estimate Euler angles (rad ) and angular velocities ( rad /s) from gyroscope data |
| yvesyuzo | 1:579511e9f0b8 | 39 | void estimate_gyro () ; |
| yvesyuzo | 1:579511e9f0b8 | 40 | // Euler angles ( rad) from accelerometer data |
| yvesyuzo | 1:579511e9f0b8 | 41 | float phi_accel , theta_accel ; |
| yvesyuzo | 1:579511e9f0b8 | 42 | // Euler angles ( rad) from gyroscope data |
| yvesyuzo | 1:579511e9f0b8 | 43 | float phi_gyro , theta_gyro , psi_gyro ; |
| yvesyuzo | 1:579511e9f0b8 | 44 | // Angular velocities bias ( rad /s) |
| yvesyuzo | 1:579511e9f0b8 | 45 | float p_bias , q_bias , r_bias ; |
| yvesyuzo | 1:579511e9f0b8 | 46 | |
| yvesyuzo | 1:579511e9f0b8 | 47 | }; |
| yvesyuzo | 1:579511e9f0b8 | 48 | |
| yvesyuzo | 1:579511e9f0b8 | 49 | |
| yvesyuzo | 1:579511e9f0b8 | 50 | #endif |
| yvesyuzo | 1:579511e9f0b8 | 51 | |
| yvesyuzo | 1:579511e9f0b8 | 52 | |
| yvesyuzo | 1:579511e9f0b8 | 53 | |
| yvesyuzo | 1:579511e9f0b8 | 54 | |
| yvesyuzo | 1:579511e9f0b8 | 55 |