From 3eeb4d3741c2c680fa3730f8576c37709324eed8 Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Thu, 3 Nov 2016 11:58:51 +0100
Subject: [PATCH] Remove square root from magnitude calculation on accel_gyro.c
 Motors start now at 990 instead of 1000 to prevent glitches

---
 UAV-ControlSystem/src/drivers/accel_gyro.c | 7 +++++--
 UAV-ControlSystem/src/drivers/motors.c     | 7 ++++++-
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c
index f9c40c9..bb567dc 100644
--- a/UAV-ControlSystem/src/drivers/accel_gyro.c
+++ b/UAV-ControlSystem/src/drivers/accel_gyro.c
@@ -245,6 +245,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
 
 	HAL_Delay(60);
 
+	accel->pitchAngle = 0;
+	accel->rollAngle = 0;
 
 	return true;
 }
@@ -558,14 +560,15 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
 	accel->accelZconv = smooth[2] * smooth[2] * sign[2];
 
 	/* The total magnitude of the acceleration */
-	float magnitude = sqrtf( \
+	float magnitude = ( \
 		ABS_FLOAT(accel->accelXconv) * ABS_FLOAT(accel->accelXconv) + 	\
 		ABS_FLOAT(accel->accelYconv) * ABS_FLOAT(accel->accelYconv) + 	\
 		ABS_FLOAT(accel->accelZconv) * ABS_FLOAT(accel->accelZconv) 	\
 	);
 
 	/* Everything is normal. No outer forces */
-	if (0.85 < magnitude && magnitude < 1.15)
+	if (0.81 < magnitude && magnitude < 1.21)
+	//if (false)
 	{
 		// Roll
 		accel->rollAngle = -atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI;
diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c
index 53bb569..1a44d5c 100644
--- a/UAV-ControlSystem/src/drivers/motors.c
+++ b/UAV-ControlSystem/src/drivers/motors.c
@@ -14,6 +14,7 @@
 #include "drivers/motors.h"
 #include "drivers/failsafe_toggles.h"
 #include "config/eeprom.h"
+#include "drivers/motormix.h"
 
 const int MotorPWMPeriode = 2000; //Micro seconds
 const int MotorPWMInitPulse = 1000;
@@ -162,7 +163,11 @@ void pwmEnableMotor(uint8_t motor, motorOutput motorOutput)
 **************************************************************************/
 void pwmEnableAllMotors(motorOutput motorOutput)
 {
-	 for (uint8_t i = 1; i < 11; i++ ) pwmEnableMotor(i, motorOutput);
+	 for (uint8_t i = 1; i < 11; i++ )
+	 {
+		 pwmEnableMotor(i, motorOutput);
+		 pwmAdjustSpeedOfMotor(i,mixerConfig.minCommand);
+	 }
 }
 
 /**************************************************************************