Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer
AttitudeEstimator/AttitudeEstimator.cpp
- Committer:
- fbob
- Date:
- 2018-10-03
- Revision:
- 14:7439203a9e44
- Parent:
- 13:1a871ebd35bb
- Child:
- 15:155ca63b7884
File content as of revision 14:7439203a9e44:
#include "mbed.h" #include "AttitudeEstimator.h" // Class constructor AttitudeEstimator::AttitudeEstimator() : imu(PC_9,PA_8) { } // Initialize class void AttitudeEstimator::init() { // Initialize IMU sensor object imu.init(); // Calibrate gyroscope calibrate_gyro(); } // Calibrates gyroscope by calculating angular velocity bias (rad/s) void AttitudeEstimator::calibrate_gyro() { // Reser angular velocites bias (rad/s) p_bias = 0.0f; q_bias = 0.0f; r_bias = 0.0f; // Calculate angular velocities bias (rad/s) by averaging 600 samples during 2 seconds for(int i = 0; i<600;i++) { imu.read(); p_bias += imu.gx/600.0f; q_bias += imu.gy/600.0f; r_bias += imu.gz/600.0f; wait(dt); } } // Estimate euler angles (rad) and angular velocity (rad/s) void AttitudeEstimator::estimate() { // Read IMU sensor data imu.read(); // Estimate Euler angles (rad) and angular velocities (rad/s) from accelerometer and gyroscope data estimate_accel(); estimate_gyro(); // Ponderate Euler angles (rad) estimation from accelerometer and gyroscope phi = phi_accel*rho_att + phi_gyro*(1.0f-rho_att); theta = theta_accel*rho_att + theta_gyro*(1.0f-rho_att); psi = psi_gyro; } // Estimate Euler angles (rad) from accelerometer data void AttitudeEstimator::estimate_accel() { // Estimate Euler angles (rad) phi_accel = atan2(-imu.ay,-imu.az); theta_accel = atan2(imu.ax,-((imu.az>0)-(imu.az<0))*sqrt(pow(imu.ay,2)+pow(imu.az,2))); } // Estimate Euler angles (rad) and angular velocities (rad/s) from gyroscope data void AttitudeEstimator::estimate_gyro() { // Estimate angular velocities (rad/s) p = imu.gx - p_bias; q = imu.gy - q_bias; r = imu.gz - r_bias; // Estimate Euler angles (rad) phi_gyro = phi + (p+q*sin(phi)*tan(theta)+r*cos(phi)*tan(theta))*dt; theta_gyro = theta + (q*cos(phi)-r*sin(phi))*dt; psi_gyro = psi + (q*sin(phi)/cos(theta)+r*cos(phi)/cos(theta))*dt; // Adjust Euler angles (rad) to be in the interval of +/- pi if(phi_gyro > PI) { phi_gyro = phi_gyro - 2*PI; } else if(phi_gyro < -PI) { phi_gyro = phi_gyro + 2*PI; } if(theta_gyro > PI) { theta_gyro = theta_gyro - 2*PI; } else if(theta_gyro < -PI) { theta_gyro = theta_gyro + 2*PI; } if(psi_gyro > PI) { psi_gyro = psi_gyro - 2*PI; } else if(psi_gyro < -PI) { psi_gyro = psi_gyro + 2*PI; } }