controlador de atitude
Diff: AttitudeEstimator/AttitudeEstimator.cpp
- Branch:
- yuri
- Revision:
- 8:c96125e9ac70
- Parent:
- 1:579511e9f0b8
--- a/AttitudeEstimator/AttitudeEstimator.cpp Wed Oct 17 12:18:29 2018 +0000 +++ b/AttitudeEstimator/AttitudeEstimator.cpp Wed Nov 21 10:07:00 2018 +0000 @@ -26,14 +26,17 @@ q_bias = 0.0f; r_bias = 0.0f; - for(count = 1; count <= taxa; ++count) + for(count = 1; count <= taxa; count++) { imu.read(); - p_bias += imu.gx/200.0f; - q_bias += imu.gy/200.0f; - r_bias += imu.gz/200.0f; + p_bias += imu.gx; + q_bias += imu.gy; + r_bias += imu.gz; wait(0.005); } + p_bias = p_bias/200.0f; + q_bias = q_bias/200.0f; + r_bias = r_bias/200.0f; } // Estimate Euler angles (rad ) from accelerometer data @@ -92,10 +95,7 @@ imu.read(); estimate_accel(); estimate_gyro(); - phi = rho*phi_accel + (1-rho)*phi_gyro; - theta = rho*theta_accel + (1-rho)*theta_gyro; + phi = rho*phi_accel + (1.0f-rho)*phi_gyro; + theta = rho*theta_accel + (1.0f-rho)*theta_gyro; psi = psi_gyro; -} - - - +} \ No newline at end of file