philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Successful tests from friday. This code is not correct though. Regards ACC!!! Even though in baro2 branch

Browse Source
This commit is contained in:
philsson 2016-11-21 09:03:08 +01:00
parent 4986238d1e
commit f6f6f50cdf
1 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
UAV-ControlSystem/src/drivers/accel_gyro.c
View File

@ -581,7 +581,8 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
accel->pitchAngle += deltaGyroAngleFloat[0];
//If the g forces of the accelerometer is within the given magnitude we will also add accelerometer data to the calculation
if (Low_Magnitude < magnitude && magnitude < High_Magnitude)
//if (Low_Magnitude < magnitude && magnitude < High_Magnitude)
if(0)
{
//Calculate the pure angle given by the accelerometer data
float a_RollAngle = -atan2(accelConv[0], sqrt(accelConv[1]*accelConv[1] + accelConv[2]*accelConv[2]))*180/M_PI;
@ -593,8 +594,11 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
// accel->pitchAngle += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR;
accel->rollAngle += (a_RollAngle/4 - accel->rollAngle/GYRO_ACC_DIV_FACTOR);
accel->pitchAngle += (a_PitchAngle/4 - accel->pitchAngle/GYRO_ACC_DIV_FACTOR);
// accel->rollAngle += (a_RollAngle/4 - accel->rollAngle/GYRO_ACC_DIV_FACTOR);
// accel->pitchAngle += (a_PitchAngle/4 - accel->pitchAngle/GYRO_ACC_DIV_FACTOR);
accel->rollAngle = (a_RollAngle);
accel->pitchAngle = (a_PitchAngle);
}
//Always constran the angular values within the possible ranges 90 to -90