Page 162 - 만들면서 배우는 아두이노 드론

float accel_x, accel_y, accel_z;
float accel_xz, accel_yz;
const float RADIANS_TO_DEGREES = 180 / 3.14159;

accel_x = AcX - baseAcX;
accel_z = AcY - baseAcY;
accel_z = AcZ + (16384 - baseAcZ);

accel_yz = sqrt(pow(accel_y, 2) + pow(accel_z, 2));
accel_angle_y = atan(-accel_x / accel_yz) * RADIANS_TO_DEGREES;

accel_xz = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
accel_angle_x = atan(accel_y / accel_xz) * RADIANS_TO_DEGREES;

accel_angle_z = 0;
}

void calcGyroYPR() {
const float GYROXYZ_TO_DEGREES_PER_SEC = 131;

gyro_x = (GyX - baseGyX) / GYROXYZ_TO_DEGREES_PER_SEC;
gyro_y = (GyY - baseGyY) / GYROXYZ_TO_DEGREES_PER_SEC;
gyro_z = (GyZ - baseGyZ) / GYROXYZ_TO_DEGREES_PER_SEC;

gyro_angle_x += gyro_x * dt;
gyro_angle_y += gyro_y * dt;
gyro_angle_z += gyro_z * dt;
}

void calcFilteredYPR() {
const float ALPHA = 0.96;
float tmp_angle_x, tmp_angle_y, tmp_angle_z;

tmp_angle_x = filtered_angle_x + gyro_x * dt;
tmp_angle_y = filtered_angle_y + gyro_y * dt;
tmp_angle_z = filtered_angle_z + gyro_z * dt;

filtered_angle_x = ALPHA * tmp_angle_x + (1.0 - ALPHA) * accel_angle_x;
filtered_angle_y = ALPHA * tmp_angle_y + (1.0 - ALPHA) * accel_angle_y;

161