diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c
index 737946c..9fb8d48 100644
--- a/UAV-ControlSystem/src/drivers/I2C.c
+++ b/UAV-ControlSystem/src/drivers/I2C.c
@@ -34,8 +34,8 @@ bool i2c_configure(I2C_TypeDef *i2c,
 		i2c_port = I2C1_PORT;
 		sda_pin = I2C1_SDA_PIN;
 		scl_pin = I2C1_SCL_PIN;
-		if(__HAL_RCC_I2C1_IS_CLK_DISABLED())
-			__HAL_RCC_I2C1_CLK_ENABLE();
+//		if(__HAL_RCC_I2C1_IS_CLK_DISABLED())
+//			__HAL_RCC_I2C1_CLK_ENABLE();
 	}
 	else if(i2c == I2C2)
 	{
@@ -55,15 +55,18 @@ bool i2c_configure(I2C_TypeDef *i2c,
 	//Initialize pins for SCL and SDA
 	GPIO_InitTypeDef GPIO_InitStruct;
 	GPIO_InitStruct.Pin = sda_pin | scl_pin;
-	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD/*GPIO_MODE_AF_PP*/;
-	GPIO_InitStruct.Pull = GPIO_NOPULL/*GPIO_PULLUP*/;
+	GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
+	GPIO_InitStruct.Pull = GPIO_NOPULL;
 	GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
 	GPIO_InitStruct.Alternate = i2c_af;
 	HAL_GPIO_Init(i2c_port, &GPIO_InitStruct);
 
+	HAL_Delay(10);
+	if(__HAL_RCC_I2C1_IS_CLK_DISABLED())
+				__HAL_RCC_I2C1_CLK_ENABLE();
 	//Initialize I2C communication
 	out_profile->Instance = i2c;
-	out_profile->Init.ClockSpeed = 400000;
+	out_profile->Init.ClockSpeed = 100000;
 	out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2/*I2C_DUTYCYCLE_2*/;
 	out_profile->Init.OwnAddress1 = my_address;
 	out_profile->Init.OwnAddress2 = 0;
diff --git a/UAV-ControlSystem/src/drivers/barometer.c b/UAV-ControlSystem/src/drivers/barometer.c
index e7f1d5a..93590fe 100644
--- a/UAV-ControlSystem/src/drivers/barometer.c
+++ b/UAV-ControlSystem/src/drivers/barometer.c
@@ -367,6 +367,18 @@ bool barometer_reset()
 		coefficients_arr[i] = rxbuf[0] << 8 | rxbuf[1];
 	}
 
+	uint8_t cobuf2[3] = {0};
+	cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + CMD_ADC_4096);
+	HAL_GPIO_DeInit(I2C1_PORT, I2C1_SCL_PIN);
+	HAL_GPIO_DeInit(I2C1_PORT, I2C1_SDA_PIN);
+	i2c_configure(I2C1, &baroI2C_handle, 0x0);
+	bool isSent = i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
+	HAL_Delay(20);
+
+	cobuf2[0] = CMD_ADC_READ;
+	i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
+	i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf2, 3);
+
 	/* Read values and get a calibration value for height */
 	/* Run loop 5 times since there are 5 state, also need delay to ensure values will be read */
 	for (int i = 0; i < 5; i ++)
@@ -442,26 +454,34 @@ void barometer_CaclulateValues()
 	switch (currentCalculationState)
 	{
 		case CALCSTATE_D2_CALCULATION:
-			TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D2 + CMD_ADC_4096),1); // send conversion command
+			//TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D2 + CMD_ADC_4096),1); // send conversion command
+			cobuf[0] = CMD_ADC_CONV + (CMD_ADC_D2 + CMD_ADC_4096);
+			i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
 			currentCalculationState = CALCSTATE_D2_READ;	//change the state so we will go to D2 read next time function is called
 			break;
 		case CALCSTATE_D2_READ:
-			//TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_READ, 1);	//Tell the sensor we want to read
-			//TEST_i2cRead(&baroI2C_handle, ADDRESS_BARO, cobuf, 3, 1000);		//Read the adc values
 			startTime = clock_get_us();
-			TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command
+			//TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command
+			cobuf[0] = CMD_ADC_READ;
+			i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
+			i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3);
 			endTime = clock_get_us() - startTime;
 			D2 = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2];		//Shift the values to the correct position for the 24 bit D2 value
 			currentCalculationState = CALCSTATE_D1_CALCULATION;
 			break;
 		case CALCSTATE_D1_CALCULATION:
-			TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D1 + CMD_ADC_4096),1); // send conversion command
+			//TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D1 + CMD_ADC_4096),1); // send conversion command
+			cobuf[0] = CMD_ADC_CONV + (CMD_ADC_D1 + CMD_ADC_4096);
+			i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
 			currentCalculationState = CALCSTATE_D1_READ;	//change the state so we will go to D1 read next time function is called
 			break;
 		case CALCSTATE_D1_READ:
 			//TEST_i2cWrite(&baroI2C_handle, ADDRESS_BARO, CMD_ADC_READ, 1);	//Tell the sensor we want to read
 			//HAL_I2C_Master_Receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3, 1000);		//Read the adc values
-			TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command
+			//TEST_i2cRead(I2C1, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command
+			cobuf[0] = CMD_ADC_READ;
+			i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
+			i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3);
 			D1 = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2];		//Shift the values to the correct position for the 24 bit D2 value
 			currentCalculationState = CALCSTATE_CALCULATE_PTA;
 			break;