Data.Roll -= ((float)Data.Gyro_Raw[0] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the Y-axis Data.Pitch += ((float)Data.Gyro_Raw[1] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis Data.Yaw += ((float)Data.Gyro_Raw[2] / GYROSCOPE_SENSITIVITY) * dt; roll = sqrt(((float)Data.Accel_Raw[0]*(float)Data.Accel_Raw[0])+((float)Data.Accel_Raw[2]*(float)Data.Accel_Raw[2])); pitch = sqrt(((float)Data.Accel_Raw[1]*(float)Data.Accel_Raw[1])+((float)Data.Accel_Raw[2]*(float)Data.Accel_Raw[2])); yaw = sqrt(((float)Data.Accel_Raw[0]*(float)Data.Accel_Raw[0])+((float)Data.Accel_Raw[1]*(float)Data.Accel_Raw[1])); int forceMagnitudeApprox = abs(Data.Accel_Raw[0]) + abs(Data.Accel_Raw[1]) + abs(Data.Accel_Raw[2]); if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768) { // Turning around the Y axis results in a vector on the X-axis rollAcc = atan2f((float)Data.Accel_Raw[1], roll) * 180 / M_PI; Data.Roll = (Data.Roll) * 0.98f + rollAcc * 0.02f; cout <<Data.Roll<<"\t"; // Turning around the X axis results in a vector on the Y-axis pitchAcc = atan2f((float)Data.Accel_Raw[0], pitch) * 180 / M_PI; Data.Pitch = (Data.Pitch) * 0.98f + pitchAcc * 0.02f; cout <<Data.Pitch<<"\t"; yawAcc = atan2f(yaw, (float)Data.Accel_Raw[2]) * 180 / M_PI; Data.Yaw = (Data.Yaw) * 0.98f + yawAcc * 0.02f; cout <<Data.Yaw<<"\t\n"; }
var
This content, along with any associated source code and files, is licensed under The Code Project Open License (CPOL)