Improved angle update to accelerometer

UAV-ControlSystem/inc/Flight/pid.h

@@ -84,6 +84,8 @@ void pidInit();
 **************************************************************************/
 void pidRun(uint8_t ID);
 
+void readAcc(void);
+
 void pidEproom(void);
 
 #endif /* FLIGHT_PID_H_ */

UAV-ControlSystem/src/Flight/pid.c

@@ -31,7 +31,7 @@
 
 #define RADIO_RANGE 500												/*Radio range input*/
 #define BAROMETER_RANGE 10											/*Determines the range of the maximum height (limits the rc input)*/
-#define ACCELEROMETER_RANGE 60										/*Determines the range of the maximum angle  (limits the rc input) & (Accelerometer takes int to max 16 G)*/
+#define ACCELEROMETER_RANGE 30										/*Determines the range of the maximum angle  (limits the rc input) & (Accelerometer takes int to max 16 G)*/
 #define GYRO_RANGE 720												/*Determines the maximum rotational limit (limits the rc input)*/
 #define COMPASS_RANGE 180											/*Determines the maximum compass limit (limits the rc input)*/
 
@@ -69,7 +69,7 @@ float accRollFineTune = 0;
 float accPitchFineTune = 0;
 
 float throttleRate = 1;
-int HoverForce = 1475;/*Struct profile for input data from sensor*/
+int HoverForce = 1475;								/*Struct profile for input data from sensor*/
 
 /**************************************************************************
 * BRIEF: Scales data from input range to output range                     *
@@ -109,8 +109,6 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
 		break;
 	case PID_ID_ACCELEROMETER:
 
-		mpu6000_read_angle(&accelProfile,&gyroProfile);	/*Reads data from accelerometer*/
-
 		sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune;
 		sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune;
 
@@ -245,7 +243,6 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
 		}
 	}
 
-
 	/* -----calculate I component ---- */
 	float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis];
 
@@ -341,7 +338,6 @@ void pidAccelerometer(void)
 		PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
 		counterAcc +=1;
 	}
-
 }
 
 /**************************************************************************
@@ -418,6 +414,12 @@ void pidRun(uint8_t ID)
 	}
 }
 
+void readAcc(void)
+{
+	/*Reads data from accelerometer*/
+	mpu6000_read_angle(&accelProfile,&gyroProfile);
+}
+
 /*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/
 
 /**************************************************************************

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^4) //  that means a CMP_FACTOR of 1024 (2^10)
+#define GYRO_ACC_DIV_FACTOR (2^16) //  that means a CMP_FACTOR of 1024 (2^10)
 #define ACCEL_LPF_FACTOR	16
 
 #define GetMagnitude(x)		(x*x)
@@ -588,16 +588,8 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
 		float a_PitchAngle	 = atan2( accelConv[1], sqrt(accelConv[2]*accelConv[2] + accelConv[0]*accelConv[0]))*180/M_PI;
 
 		//Check how much the accelerometer angle differs from the current calculated ange with the gyro. Add calculated factor to the real angle value
-// TODO: This is the original code. Test to set factors for them separatelly
+		accel->rollAngle 	 += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR;
-//		accel->rollAngle 	 += (a_RollAngle - accel->rollAngle) / GYRO_ACC_DIV_FACTOR;
+		accel->pitchAngle	 += (a_PitchAngle - accel->pitchAngle) / GYRO_ACC_DIV_FACTOR;
-//		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);
 	}
 
-	//Always constran the angular values within the possible ranges 90 to -90
-	accel->rollAngle = constrainf(accel->rollAngle, -90,90);
-	accel->pitchAngle = constrainf(accel->pitchAngle, -90,90);
 }

UAV-ControlSystem/src/tasks_main.c

@@ -46,18 +46,10 @@
 
 void systemTaskGyroPid(void)
 {
-	//Read gyro and update PID and finally update the motors. The most important task in the system
-
-	//Update Gyro
-
-	//Convert?
 
 	//PID Gyro
 	pidRun(PID_ID_GYRO);
 
-	//MIX GO
-
-
 	//call the motor mix
 	mix();
 }
@@ -65,12 +57,8 @@ void systemTaskGyroPid(void)
 void systemTaskAccelerometer(void)
 {
 
+	readAcc();
 	pidRun(PID_ID_ACCELEROMETER);
-	//update the accelerometer data
-//	uint8_t c = 97;
-//	usart_transmit(&cliUsart, &c, 1, 1000000000);
-
-
 }
 
 void systemTaskAttitude(void)