diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c
index 18ceb86..fbb1f94 100644
--- a/UAV-ControlSystem/src/Flight/pid.c
+++ b/UAV-ControlSystem/src/Flight/pid.c
@@ -26,7 +26,7 @@
 
 #define PTERM_SCALE 0.032029f										/*P-term used as a scale value to the PID controller*/
 #define ITERM_SCALE 0.0012f											/*I-term used as a scale value to the PID controller*/
-#define DTERM_SCALE 0.000529f										/*D-term used as a scale value to the PID controller*/
+#define DTERM_SCALE 0.0529f										/*D-term used as a scale value to the PID controller*/
 #define BAROMETER_SCALE 5
 
 #define RADIO_RANGE 500												/*Radio range input*/
@@ -335,15 +335,13 @@ void pidAccelerometer(void)
 
 	pidUAV(&PidProfile[PID_ID_ACCELEROMETER], &PidProfileBuff[PID_ID_ACCELEROMETER]);
 
-	if (counterAcc > 50)
+	if (counterAcc < 80)
 	{
 		PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll;
 		PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
-	}
-	else
-	{
 		counterAcc +=1;
 	}
+
 }
 
 /**************************************************************************
@@ -406,9 +404,11 @@ void pidRun(uint8_t ID)
 		else
 		{
 			pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]);
+			PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -20, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
+
 
 			//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -15, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
-			PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
+			//PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)PidProfile[PID_ID_BAROMETER].pid_out_limit, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
 		}
 
 		break;
@@ -531,8 +531,8 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
 		PidProfile[ID].D[PITCH] = 0;
 		PidProfile[ID].D[YAW] = 0;
 
-		PidProfile[ID].PIDweight[ROLL] = 4;
-		PidProfile[ID].PIDweight[PITCH] = 4;
+		PidProfile[ID].PIDweight[ROLL] = 100;
+		PidProfile[ID].PIDweight[PITCH] = 100;
 		PidProfile[ID].PIDweight[YAW] = 100;
 
 		PidProfile[ID].pidEnabled = true;
@@ -590,6 +590,8 @@ void pidEproom(void)
 {
 	PidProfile[PID_ID_BAROMETER].PIDweight[ROLL] = 200;
 
+	PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 20;
+	PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 20;
 	PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100;
 
 	PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //�NDRA TILL SEKUNDER inte ms
diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c
index 471f835..8bd75e8 100644
--- a/UAV-ControlSystem/src/drivers/accel_gyro.c
+++ b/UAV-ControlSystem/src/drivers/accel_gyro.c
@@ -526,7 +526,7 @@ bool mpu6000_who_am_i()
 
 /* Set the Gyro Weight for Gyro/Acc complementary filter
    Increasing this value would reduce and delay Acc influence on the output of the filter*/
-#define GYRO_ACC_DIV_FACTOR (2^8) //  that means a CMP_FACTOR of 1024 (2^10)
+#define GYRO_ACC_DIV_FACTOR (2^4) //  that means a CMP_FACTOR of 1024 (2^10)
 
 #define GetMagnitude(x)		(x*x)
 #define Low_Magnitude		(GetMagnitude(0.85))
diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c
index 25ba171..e296f2d 100644
--- a/UAV-ControlSystem/src/drivers/motormix.c
+++ b/UAV-ControlSystem/src/drivers/motormix.c
@@ -103,7 +103,7 @@ void mix()
 	int16_t RPY_Mix[MOTOR_COUNT];		// Roll Pitch and Yaw variables array
 	int16_t RPY_Mix_Min = 0; 			// Stores the minimum desired command for any motor
 	int16_t RPY_Mix_Max = 0;			// Maximum desired command for any motor
-	int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]*throttleRate;
+	int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]/**throttleRate*/;
 
 	if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += HoverForce;
 
diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c
index e596fa9..38610a8 100644
--- a/UAV-ControlSystem/src/tasks_main.c
+++ b/UAV-ControlSystem/src/tasks_main.c
@@ -69,6 +69,8 @@ void systemTaskAccelerometer(void)
 	//update the accelerometer data
 //	uint8_t c = 97;
 //	usart_transmit(&cliUsart, &c, 1, 1000000000);
+
+
 }
 
 void systemTaskAttitude(void)
@@ -254,6 +256,10 @@ void systemTaskSonar(void)
 
 void systemTaskAltitude(void)
 {
+	uint8_t c[50];
+		sprintf(c, "Roll: %-6f, Pitch %-6f\r", accelProfile.rollAngle, accelProfile.pitchAngle);
+		usart_transmit(&cliUsart, &c, 50, 1000000000);
+
 	//Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar
 
 	//double temperature = barometer_GetCurrentTemperature();