From f6f6f50cdfad0299ae769f0ce438b1b7d4ced4cd Mon Sep 17 00:00:00 2001 From: philsson Date: Mon, 21 Nov 2016 09:03:08 +0100 Subject: [PATCH] Successful tests from friday. This code is not correct though. Regards ACC!!! Even though in baro2 branch --- UAV-ControlSystem/src/drivers/accel_gyro.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c index 2a40d42..ad788e8 100644 --- a/UAV-ControlSystem/src/drivers/accel_gyro.c +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -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