started calibration fixing

UAV-ControlSystem/src/config/cli.c

@@ -73,6 +73,9 @@ typedef enum
 	ACTION_RESET,					//Resets the entire eeprom
 	ACTION_STATS,					//gives statistics on the system
 	ACTION_MOTORCALIBRATE,
+	ACTION_GYROCALIBRATE,
+	ACTION_ACCELEROMTETERCALIBRATE,
+	ACTION_COMPASSCALIBRATE,
 
 	//The number of actions
 	ACTION_COUNT,					//Count of the number of actions
@@ -94,7 +97,10 @@ const typeString commandAction_Strings[ACTION_COUNT] = {
 	"reboot",
 	"reset",
 	"stats",
-	"motorcalibrate"
+	"calibrate_motors",
+	"calibrate_gyro",
+	"calibrate_accelerometer",
+	"calibrate_compass"
 };
 
 /* String values descrbing information of a certain action */
@@ -110,8 +116,11 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = {
 	"| exit - Exits the CLI mode.\n\r",
 	"| reboot - Exit CLI and reboots the system.\n\r",
 	"| reset - Restore all the values to its default values.\n\r",
-	"| stats - Gives some current stats of the system and tasks.\n\r"
+	"| stats - Gives some current stats of the system and tasks.\n\r",
-	"| motorcalibrate - Calibrates all motors."
+	"| calibrate_motors - Calibrates all motors.",
+	"| calibrate_gyro - Calibrates the gyro.",
+	"| calibrate_accelerometer - Calibrates the accelerometer.",
+	"| calibrate_compass - Calibrates the compass."
 };
 
 /* String array containing all the signature examples for each action */
@@ -128,7 +137,10 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = {
 	" reboot",
 	" reset",
 	" stats",
-	" motorcalibrate"
+	" calibrate_motors",
+	" calibrate_gyro",
+	" calibrate_accelerometer",
+	" calibrate_compass"
 };
 
 /* Size values for the values a command will require */
@@ -1422,12 +1434,55 @@ void cliRun()
 					{
 						TransmitBack("Starting calibration, BE CAREFUL!!! \n\n\r");
 						calibrateMotors();
+						TransmitBack("Wait until the beeping have ceased... \n\n\r");
 					}
 					else
 					{
 						TransmitBack("Exiting the calibration, BE CAREFUL!!! \n\n\r");
 					}
 
+					break;
+			case commandMask(commandSize_1, ACTION_GYROCALIBRATE):
+					TransmitBack("Do you really want to calibrate the gyro? (y/n)\n\n\r");
+					if (ReceiveBinaryDecision(121,110))
+					{
+						TransmitBack("Starting calibration! \n\n\r");
+						TransmitBack("NOT IMPLEMENTED! \n\n\r");
+						TransmitBack("Calibration complete! \n\n\r");
+					}
+					else
+					{
+						TransmitBack("Exiting the calibration! \n\n\r");
+					}
+
+					break;
+			case commandMask(commandSize_1, ACTION_ACCELEROMTETERCALIBRATE):
+					TransmitBack("Do you really want to calibrate the accelerometer? (y/n)\n\n\r");
+					if (ReceiveBinaryDecision(121,110))
+					{
+						TransmitBack("Starting calibration! \n\n\r");
+						TransmitBack("NOT IMPLEMENTED! \n\n\r");
+						TransmitBack("Calibration complete! \n\n\r");
+					}
+					else
+					{
+						TransmitBack("Exiting the calibration! \n\n\r");
+					}
+
+					break;
+			case commandMask(commandSize_1, ACTION_COMPASSCALIBRATE):
+					TransmitBack("Do you really want to calibrate the compass? (y/n)\n\n\r");
+					if (ReceiveBinaryDecision(121,110))
+					{
+						TransmitBack("Starting calibration! \n\n\r");
+						TransmitBack("NOT IMPLEMENTED! \n\n\r");
+						TransmitBack("Calibration complete! \n\n\r");
+					}
+					else
+					{
+						TransmitBack("Exiting the calibration! \n\n\r");
+					}
+
 					break;
 			default:
 				if (actionId != ACTION_NOACTION)

UAV-ControlSystem/src/drivers/accel_gyro.c

@@ -44,7 +44,7 @@ bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32
 
 /***********************************************************************
  * BRIEF: mpu6000_read_offset reads and returns the offset of the       *
- * gyroscope and accelerometer                                          *
+ * gyroscope                                                            *
  * INFORMATION:                                                         *
  * Is automatically called when mpu6000_init is called                  *
  * The flight controller needs to be stationary when this function is   *
@@ -52,46 +52,104 @@ bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32
  * When the UAV is finished this data could be saved so that the        *
  * offset doesn't need to be read every time                            *
  ***********************************************************************/
-HAL_StatusTypeDef mpu6000_read_offset(gyro_t* gyro, accel_t* accel)
+HAL_StatusTypeDef mpu6000_read_gyro_offset(gyro_t* gyro)
 {
 	uint8_t dataG[6]; /* Temporary data variable used to receive gyroscope data */
-	uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */
+	int16_t dataGCheck[6] = {0};	/* checkval */
+	uint8_t maxDrift = 5;
+	bool calibrate_go = true;
+	bool calibrate_ok = false;
+	int countCalibrate = 0;
 
-	if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10))
+	//small delay so that we know the gyro should give some values
-		return HAL_ERROR;
+	HAL_Delay(100);
+	for (int i = 0; i < 100; i++)
+	{
+		//assume that we should calibrate at the start of each loop
+		calibrate_go = true;
 
+		//get new values
 		if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10))
 				return HAL_ERROR;
 
+		//cheack the new values and see if they are within acceptable range compared to the previous values, so that it is realativelly still
+		for(int j = 0; j < 6; j ++)
+		{
+			int16_t currentDrift = (int16_t)abs(dataGCheck[j] - dataG[j]);
+			if (!(currentDrift < maxDrift))
+				calibrate_go = false;
+
+			dataGCheck[j] = dataG[j];
+		}
+
+		//if true, we have good values exit loop
+		if (calibrate_go)
+		{
+			countCalibrate++;
+		}
+		if (countCalibrate > 4)
+		{
+			calibrate_ok = true;
+			break;
+		}
+		//wait for a little bit so the checks are not to fast
+		HAL_Delay(200);
+	}
+
+
 #ifdef YAW_ROT_0
 	gyro->offsetX = -(((int16_t)dataG[0] << 8) | dataG[1]);
 	gyro->offsetY = -(((int16_t)dataG[2] << 8) | dataG[3]);
 	gyro->offsetZ =  (((int16_t)dataG[4] << 8) | dataG[5]);
 
-	accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1];
-	accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3];
-	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
 #elif defined(YAW_ROT_90)
 	gyro->offsetX = -(((int16_t)dataG[2] << 8) | dataG[3]);
 	gyro->offsetY =  (((int16_t)dataG[0] << 8) | dataG[1]);
 	gyro->offsetZ =  (((int16_t)dataG[4] << 8) | dataG[5]);
-
-	accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3];
-	accel->offsetY =  ((int16_t)dataA[0] << 8) | dataA[1];
-	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
 #elif defined(YAW_ROT_180)
 	gyro->offsetX = (((int16_t)dataG[0] << 8) | dataG[1]);
 	gyro->offsetY = (((int16_t)dataG[2] << 8) | dataG[3]);
 	gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]);
-
-	accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1];
-	accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3];
-	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
 #elif defined(YAW_ROT_270)
 	gyro->offsetX =  (((int16_t)dataG[2] << 8) | dataG[3]);
 	gyro->offsetY = -(((int16_t)dataG[0] << 8) | dataG[1]);
 	gyro->offsetZ =  (((int16_t)dataG[4] << 8) | dataG[5]);
+#endif
 
+	return HAL_OK;
+}
+
+/***********************************************************************
+ * BRIEF: mpu6000_read_offset reads and returns the offset of the       *
+ * accelerometer                                                        *
+ * INFORMATION:                                                         *
+ * Is automatically called when mpu6000_init is called                  *
+ * The flight controller needs to be stationary when this function is   *
+ * called                                                               *
+ * When the UAV is finished this data could be saved so that the        *
+ * offset doesn't need to be read every time                            *
+ ***********************************************************************/
+HAL_StatusTypeDef mpu6000_read_acc_offset(accel_t* accel)
+{
+	uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */
+
+	if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10))
+		return HAL_ERROR;
+
+
+#ifdef YAW_ROT_0
+	accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1];
+	accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3];
+	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
+#elif defined(YAW_ROT_90)
+	accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3];
+	accel->offsetY =  ((int16_t)dataA[0] << 8) | dataA[1];
+	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
+#elif defined(YAW_ROT_180)
+	accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1];
+	accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3];
+	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
+#elif defined(YAW_ROT_270)
 	accel->offsetX =  ((int16_t)dataA[2] << 8) | dataA[3];
 	accel->offsetY = -((int16_t)dataA[0] << 8) | dataA[1];
 	accel->offsetZ =  accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]);
@@ -180,7 +238,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
 	if(!mpu6000_transmit(reg, 2))
 		return false;
 
-	mpu6000_read_offset(gyro, accel);
+	mpu6000_read_gyro_offset(gyro);
+	mpu6000_read_acc_offset(accel);
 
 	return true;
 }

UAV-ControlSystem/src/tasks_main.c

@@ -183,7 +183,7 @@ void systemTaskBattery(void)
 
 void systemTaskBaro(void)
 {
-	barometer_CaclulateValues();
+	//barometer_CaclulateValues();
 
 
 
@@ -261,10 +261,10 @@ void systemTaskSonar(void)
 void systemTaskAltitude(void)
 {
 	//Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar
-	double temperature = barometer_GetCurrentTemperature();
+	/*double temperature = barometer_GetCurrentTemperature();
 	char buffer[50];
 	sprintf(buffer, "Temperature: %8.2f \n\r", temperature);
-	usart_transmit(&cliUsart, buffer, 50, 1000000000);
+	usart_transmit(&cliUsart, buffer, 50, 1000000000);*/
 }
 
 void systemTaskBeeper(void)