--no commit message

Imanol-Mikel Barba Sabariego
1 parent e08f5763
Showing 7 changed files with 145 additions and 115 deletions
Project/applications/smartcities/i2c.c
Project/applications/smartcities/include/i2c.h
Project/applications/smartcities/include/json.h
Project/applications/smartcities/include/sensors.h
Project/applications/smartcities/json.c
Project/applications/smartcities/main2.c
Project/applications/smartcities/sensors.c
@@ -27,6 +27,9 @@ void I2C_init(void)
 	I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; // set address length to 7 bit addresses
 	I2C_Init(I2C1, &I2C_InitStruct);				// init I2C1
+	//Enable clock stretching
+	I2C_StretchClockCmd(I2C1, ENABLE);
+
 	// enable I2C1
 	I2C_Cmd(I2C1, ENABLE); //sets PE bit in CR1, at end`
 }
@@ -38,18 +41,53 @@ void I2C_start(I2C_TypeDef* I2Cx, uint8_t address, uint8_t direction)
 	{
 		// wait until I2C1 is not busy any more
 	}
+	printf("I2C ready\r\n");
+	I2C_GenerateSTART(I2Cx, ENABLE);
+	printf("Start sent\r\n");
+	while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT))
+	{
+		// wait for I2C1 EV5 --> Slave has acknowledged start condition
+	}
+	printf("Start ACK\r\n");
+	I2C_Send7bitAddress(I2Cx, address, direction);
+	printf("Master -> Slave Address and R/W sent\r\n");
+	if(direction == I2C_Direction_Transmitter)
+	{
+		while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
+		{
+			// wait for I2Cx EV6, check if Slave has acknowledged Master transmitter mode
+		}
+		printf("Slave -> Master ACK\r\n");
+	}
+	else if(direction == I2C_Direction_Receiver)
+	{
+		while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED))
+		{
+			// wait for I2Cx EV6, check if Slave has acknowledged Master receiver mode
+		}
+		printf("Slave -> Master ACK\r\n");
+	}
+}
+
+void I2C_restart(I2C_TypeDef* I2Cx, uint8_t address, uint8_t direction)
+{
+	printf("Sending repeated I2C Start...\r\n");
 	I2C_GenerateSTART(I2Cx, ENABLE);
+	printf("Start sent\r\n");
 	while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_MODE_SELECT))
 	{
 		// wait for I2C1 EV5 --> Slave has acknowledged start condition
 	}
+	printf("Start ACK\r\n");
 	I2C_Send7bitAddress(I2Cx, address, direction);
+	printf("Master -> Slave Address and R/W sent\r\n");
 	if(direction == I2C_Direction_Transmitter)
 	{
 		while(!I2C_CheckEvent(I2Cx, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED))
 		{
 			// wait for I2Cx EV6, check if Slave has acknowledged Master transmitter mode
 		}
+		printf("Slave -> Master ACK\r\n");
 	}
 	else if(direction == I2C_Direction_Receiver)
 	{
@@ -57,6 +95,7 @@ void I2C_start(I2C_TypeDef* I2Cx, uint8_t address, uint8_t direction)
 		{
 			// wait for I2Cx EV6, check if Slave has acknowledged Master receiver mode
 		}
+		printf("Slave -> Master ACK\r\n");
 	}
 }
@@ -68,6 +107,7 @@ void I2C_write(I2C_TypeDef* I2Cx, uint8_t data)
 	{
 		// wait for I2C1 EV8_2 --> last byte is transmitted
 	}
+	printf("Data sent\r\n");
 }
 uint8_t I2C_read_ack(I2C_TypeDef* I2Cx)
@@ -105,6 +145,7 @@ void I2C_stop(I2C_TypeDef* I2Cx)
 	{
 		// wait for I2C1 EV8_2 --> byte has been transmitted
 	}*/
+	printf("Stop sent\r\n");
 }
 uint8_t I2C_check(I2C_TypeDef* I2Cx, uint8_t address)
@@ -126,34 +167,34 @@ uint8_t I2C_check(I2C_TypeDef* I2Cx, uint8_t address)
 		return 1;
 	}
 	printf("I2C address detected: %x\r\n", address);
-	I2C_write(I2C1,0x00);
-	I2C_stop(I2C1);
+	I2C_write(I2Cx,0x00);
+	I2C_stop(I2Cx);
 	return 0;
 }
-void I2C_reset()
+void I2C_reset(I2C_TypeDef* I2Cx)
 {
 	printf("Resetting I2C...\r\n");
-	I2C_SoftwareResetCmd(I2C1, ENABLE);
-	I2C_DeInit(I2C1);
+	I2C_SoftwareResetCmd(I2Cx, ENABLE);
+	I2C_DeInit(I2Cx);
 	I2C_init();
 }
-void I2C_scan(uint8_t *addresses)
+void I2C_scan(I2C_TypeDef* I2Cx,uint8_t *addresses)
 {
 	uint8_t i, j = 0;
 	I2C_init();
 	for(i = 0; i < TOTAL_SENSORS; i++)
 	{
-		if(!I2C_check(I2C1,sensors[i]->ID << 1))
+		if(!I2C_check(I2Cx,sensors[i]->ID << 1))
 		{
 			addresses[j++] = sensors[i]->ID;
 			register_sensor(*sensors[i]);
 		}
 		else
 		{
-			I2C_reset();
+			I2C_reset(I2Cx);
 		}
 	}
 	register_sensor(battery);
@@ -15,9 +15,10 @@ void I2C_stop(I2C_TypeDef* I2Cx);
 void I2C_write(I2C_TypeDef* I2Cx, uint8_t data);
 uint8_t I2C_read_ack(I2C_TypeDef* I2Cx);
 uint8_t I2C_read_nack(I2C_TypeDef* I2Cx);
-void I2C_scan(uint8_t *addresses);
+void I2C_scan(I2C_TypeDef* I2Cx,uint8_t *addresses);
 uint8_t I2C_check(I2C_TypeDef* I2Cx, uint8_t address);
-void I2C_reset(void);
+void I2C_reset(I2C_TypeDef* I2Cx);
+void I2C_restart(I2C_TypeDef* I2Cx, uint8_t address, uint8_t direction);
 #endif
@@ -20,7 +20,7 @@
 uint8_t register_sensor(sensor sens);
 char* prepare_json_observation_statement(char** data, uint32_t nObservations);
-char* prepare_json_register_statement(module mod, sensor sens);
+char* prepare_json_register_statement(module mod, sensor sens, char *additional_info);
 char* prepare_observation(char* observation, uint32_t length);
 uint8_t send_json(char* statement, uint32_t length, char* provider_ID, char* sensor_ID);
 uint32_t find_next_index(char* string, uint32_t length, char delimiter);
@@ -24,7 +24,6 @@ typedef struct {
   char* description;
   char* type;
   char* unit;
-  char* additional_info;
 } sensor;
 void wakeup_sensors(uint8_t logic_address);
@@ -50,8 +49,6 @@ char* light_value(uint32_t light);
 //ULTRASONIC SENSOR
 uint16_t get_distance_data(void);
 void init_ultrasound(void);
-uint8_t get_distance_low(void);
-uint8_t get_distance_high(void);
 char* distance_value(uint16_t distance);
@@ -7,14 +7,15 @@ uint8_t register_sensor(sensor sens)
     char *statement;
     if(sens.ID == PRESSURE_ADDR)
     {
-        sens.additional_info = callibration_pressure_data_csv(get_pressure_callibration_data());
-        statement = prepare_json_register_statement(mod,sens);
-    	chHeapFree(sens.additional_info);
-    	sens.additional_info = NULL;
+    	printf("Putting additional info for pressure sensor...\r\n");
+    	char *callibration_data = callibration_pressure_data_csv(get_pressure_callibration_data());
+    	printf("callibration_data is: %s\r\n",callibration_data);
+        statement = prepare_json_register_statement(mod,sens,callibration_data);
+    	chHeapFree(callibration_data);
     }
     else
     {
-    	statement = prepare_json_register_statement(mod,sens);
+    	statement = prepare_json_register_statement(mod,sens,NULL);
     }
     printf("%s\r\n",statement);
 	char sensor_ID[3];
@@ -44,7 +45,7 @@ char* prepare_json_observation_statement(char** data, uint32_t nObservations)
 	return json_statement;
 }
-char* prepare_json_register_statement(module mod, sensor sens)
+char* prepare_json_register_statement(module mod, sensor sens, char *additional_info)
 {
 	printf("Registering sensor: %s in: %s\r\n",sens.description,mod.geoloc);
 	unsigned int length;
@@ -79,11 +80,15 @@ char* prepare_json_register_statement(module mod, sensor sens)
 	str_aux = join_strings(str_aux2,mod.geoloc,length,strlen(mod.geoloc),JOIN_NO_FREE);
 	chHeapFree(str_aux2);
 	length += strlen(mod.geoloc);
-	if(sens.additional_info != NULL)
+	if(additional_info != NULL)
 	{
-		str_aux2 = join_strings(str_aux,sens.additional_info,length,strlen(sens.additional_info),JOIN_NO_FREE);
+		printf("In prepare_json_register_statement: Detected additional_info\r\n");
+		str_aux2 = join_strings(str_aux,"\",\"additionalInfo\":\"",length,20,JOIN_NO_FREE);
+		chHeapFree(str_aux);
+		length += 20;
+		str_aux = join_strings(str_aux2,additional_info,length,strlen(additional_info),JOIN_NO_FREE);
 		chHeapFree(str_aux2);
-		length += strlen(sens.additional_info);
+		length += strlen(additional_info);
 	}
 	json_statement = join_strings(str_aux,"\"}]}\0",length,5,JOIN_NO_FREE);
 	chHeapFree(str_aux);
@@ -140,10 +140,10 @@ void put_buffers(char** buffers[],uint32_t ind[],uint32_t sizes[],char** cooked,
         printf("For %d and %d\n\r", ind[i],sizes[i]);
         printf("Putting data: %s\n\r",cooked[i]);
         buffers[i] = put_message(cooked[i], buffers[i] ,&ind[i],&sizes[i]);
-        printf("Message put: %s\r\n",buffers[0][ind[0]-1]);
+        printf("Message put: %s\r\n",buffers[i][ind[i]-1]);
         chHeapFree(cooked[i]);
         printf("Memory freed\n\r");
-        printf("Message put (assertion): %s\r\n",buffers[0][ind[0]-1]);
+        printf("Message put (assertion): %s with length %d\r\n",buffers[i][ind[i]-1] ,strlen(buffers[i][ind[i]-1]));
     }
 }
@@ -238,14 +238,10 @@ int main(void)
     printf("Scanning sensors...\r\n");
     //Escanea los sensores -> retorna un vector con las direcciones en cada posición del vector, si la posición del vector retorna un cero -> no existe el sensor
     //Escanea y registra
-    I2C_scan(sensors);
+    I2C_scan(I2C1,sensors);
     sensors_length=strlen((char*)sensors);
     printf("%d sensor detected...\r\n",sensors_length);
-    I2C_init();
-    printf("%s\r\n",light_value(get_light_data()));
-    printf("%s\r\n",light_value(get_light_data()));
-
     unsigned long time;
     update_time(&time);
     //desconectarwifi
@@ -262,15 +258,26 @@ int main(void)
     int i  = 1;
     while(1){
+        if(ind[0])
+        {
+            printf("Data stored in buffer: %s with index %d and size %d\r\n",buffers[0][ind[0]-1], ind[0],sizes[0]);
+        }
         time += getElapsedTime(delay);
         printf("time (absolute):\t%ul\r\ntime mod LONG_PERIOD:\t%ul\r\ntime mod SHORT_PERIOD:\t%ul\r\n",time,time%LONG_PERIOD,time%SHORT_PERIOD);
         delay = getSystemTime();
         /* Collect data from sensors */
+        if(ind[0])
+        {
+            printf("Data stored in buffer: %s with index %d and size %d\r\n",buffers[0][ind[0]-1], ind[0],sizes[0]);
+        }
         printf("Collecting data...\r\n");
         collectData(valueSensors, sensors);
         printf("Data collected...\r\n");
-
+        if(ind[0])
+        {
+            printf("Data stored in buffer: %s with index %d and size %d\r\n",buffers[0][ind[0]-1], ind[0],sizes[0]);
+        }
         time += getElapsedTime(delay);
         printf("time (absolute):\t%ul\r\ntime mod LONG_PERIOD:\t%ul\r\ntime mod SHORT_PERIOD:\t%ul\r\n",time,time%LONG_PERIOD,time%SHORT_PERIOD);
         timestamp = time;
@@ -363,7 +370,6 @@ int main(void)
 /*
  * TO-DO
  *
- * - I2C issues
  * - Test all sensors
  * - Test ADC
  * - Test register, data send and battery send
 #include "sensors.h"
-sensor light_sensor 		= {LIGHT_ADDR, 			"Light sensor", 					"illumination", 		"lux",	NULL};
-sensor ultrasound_sensor 	= {DISTANCE_ADDR, 		"Ultrasound sensor", 				"distance", 			"cm",	NULL};
-sensor pressure_sensor 		= {PRESSURE_ADDR, 		"Pressure sensor", 					"pressure", 			"hPa",	NULL};
-sensor humidity_temp_sensor = {HUMIDITY_TEMP_ADDR, 	"Temperature and humidity sensor",	"temperature,humidity",	"ºC,RH",NULL};
-sensor sound_sensor 		= {SOUND_ADDR, 			"Sound sensor",						"sound",				"mV",	NULL};
-sensor battery 				= {BATTERY_ADDR, 		"Battery Level",					"power",				"mV",	NULL};
+sensor light_sensor         = {LIGHT_ADDR,          "Light sensor",                     "illumination",         "lux"};
+sensor ultrasound_sensor    = {DISTANCE_ADDR,       "Ultrasound sensor",                "distance",             "cm"};
+sensor pressure_sensor      = {PRESSURE_ADDR,       "Pressure sensor",                  "pressure",             "hPa"};
+sensor humidity_temp_sensor = {HUMIDITY_TEMP_ADDR,  "Temperature and humidity sensor",  "temperature,humidity", "ºC,RH"};
+sensor sound_sensor         = {SOUND_ADDR,          "Sound sensor",                     "sound",                "mV"};
+sensor battery              = {BATTERY_ADDR,        "Battery Level",                    "power",                "mV"};
 sensor* sensors[TOTAL_SENSORS+1] = {&light_sensor,&ultrasound_sensor,&pressure_sensor,&humidity_temp_sensor,&sound_sensor,&battery};
@@ -92,13 +92,12 @@ uint16_t get_light_ch0(void)
 	I2C_start(I2C1,LIGHT_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0x0C);
-	I2C_stop(I2C1);
 	printf("LIGHT: Sent fetch command for CH1\r\n");
-	I2C_start(I2C1, LIGHT_ADDR << 1, I2C_Direction_Receiver);
+	I2C_restart(I2C1, LIGHT_ADDR << 1, I2C_Direction_Receiver);
  	data = I2C_read_ack(I2C1);
  	printf("LIGHT: Got low byte for CH1\r\n");
- 	data = data | (I2C_read_ack(I2C1) << 8);
+ 	data = data | (I2C_read_nack(I2C1) << 8);
  	printf("LIGHT: Got high byte for CH1\r\n");
  	I2C_stop(I2C1);	
@@ -110,13 +109,12 @@ uint16_t get_light_ch1(void)
 	I2C_start(I2C1,LIGHT_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0x0E);
-	I2C_stop(I2C1);
 	printf("LIGHT: Sent fetch command for CH1\r\n");
-	I2C_start(I2C1, LIGHT_ADDR << 1, I2C_Direction_Receiver);
+	I2C_restart(I2C1, LIGHT_ADDR << 1, I2C_Direction_Receiver);
  	data = I2C_read_ack(I2C1);
  	printf("LIGHT: Got low byte for CH1\r\n");
- 	data = data | (I2C_read_ack(I2C1) << 8);
+ 	data = data | (I2C_read_nack(I2C1) << 8);
  	printf("LIGHT: Got high byte for CH1\r\n");
  	I2C_stop(I2C1);
@@ -127,48 +125,29 @@ uint16_t get_distance_data(void)
 {
 	init_ultrasound();
 	printf("DISTANCE: Initialized\r\n");
-	uint16_t data = get_distance_high();
-	printf("Got distance high byte\r\n");
-	data = data << 8;
-	data = data | get_distance_low();
-	printf("Got distance low byte\r\n");
-	return data;
-}
-void init_ultrasound(void)
-{
-	I2C_start(I2C1,DISTANCE_ADDR << 1, I2C_Direction_Transmitter);
-	I2C_write(I2C1, 0x00);
-	I2C_stop(I2C1);
+	uint16_t data;
 	I2C_start(I2C1,DISTANCE_ADDR << 1, I2C_Direction_Transmitter);
-	I2C_write(I2C1, 0x50);
-	I2C_stop(I2C1);
+	I2C_write(I2C1, 0x02);
-	chThdSleepMilliseconds(70);
-}
-uint8_t get_distance_low(void)
-{
-	uint8_t data;
-	I2C_start(I2C1,DISTANCE_ADDR << 1, I2C_Direction_Transmitter);
-	I2C_write(I2C1, 0x03);
-	I2C_stop(I2C1);
-	
-	I2C_start(I2C1, DISTANCE_ADDR << 1, I2C_Direction_Receiver);
+	I2C_restart(I2C1, DISTANCE_ADDR << 1, I2C_Direction_Receiver);
  	data = I2C_read_ack(I2C1);
+ 	printf("DISTANCE: Got high byte\r\n");
+ 	data = data << 8;
+	data = data | I2C_read_nack(I2C1);
+	printf("DISTANCE: Got low byte\r\n");
  	I2C_stop(I2C1);
  	return data;
 }
-uint8_t get_distance_high(void)
+
+void init_ultrasound(void)
 {
-	uint8_t data;
 	I2C_start(I2C1,DISTANCE_ADDR << 1, I2C_Direction_Transmitter);
-	I2C_write(I2C1, 0x02);
+	I2C_write(I2C1, 0x00);
+	I2C_write(I2C1, 0x50);
 	I2C_stop(I2C1);
-	
-	I2C_start(I2C1, DISTANCE_ADDR << 1, I2C_Direction_Receiver);
- 	data = I2C_read_ack(I2C1);
- 	I2C_stop(I2C1);
- 	return data;
+
+	chThdSleepMilliseconds(70);
 }
 uint16_t get_pressure_data(void)
@@ -178,9 +157,8 @@ uint16_t get_pressure_data(void)
 	printf("PRESSURE: Initialized\r\n");
 	I2C_start(I2C1,DISTANCE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xF6);
-	I2C_stop(I2C1);
-
-	I2C_start(I2C1, DISTANCE_ADDR << 1, I2C_Direction_Receiver);
+	
+	I2C_restart(I2C1, DISTANCE_ADDR << 1, I2C_Direction_Receiver);
  	data = I2C_read_ack(I2C1);
  	printf("PRESSURE: Got high byte\r\n");
  	data = data << 8;
@@ -196,9 +174,8 @@ uint16_t get_pressure_temperature_data(void)
 	printf("PRESSURE: Initialized temperature\r\n");
 	I2C_start(I2C1,DISTANCE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xF6);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, DISTANCE_ADDR << 1, I2C_Direction_Receiver);
+	I2C_restart(I2C1, DISTANCE_ADDR << 1, I2C_Direction_Receiver);
  	data = I2C_read_ack(I2C1);
  	printf("PRESSURE: Got temperature high byte\r\n");
  	data = data << 8;
@@ -214,87 +191,88 @@ bmp085_callibration get_pressure_callibration_data(void)
 	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xAA);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
- 	calib_data.AC1 = I2C_read_ack(I2C1);
+	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
+ 	calib_data.AC1 = (I2C_read_ack(I2C1) << 8);
+ 	calib_data.AC1 |= I2C_read_nack(I2C1);
  	I2C_stop(I2C1);
  	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xAC);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
- 	calib_data.AC2 = I2C_read_ack(I2C1);
+	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
+ 	calib_data.AC2 = (I2C_read_ack(I2C1) << 8);
+ 	calib_data.AC2 |= I2C_read_nack(I2C1);
  	I2C_stop(I2C1);
  	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xAE);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
- 	calib_data.AC3 = I2C_read_ack(I2C1);
+	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
+ 	calib_data.AC3 = (I2C_read_ack(I2C1) << 8);
+ 	calib_data.AC3 |= I2C_read_nack(I2C1);
  	I2C_stop(I2C1);
  	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xB0);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
- 	calib_data.AC4 = I2C_read_ack(I2C1);
+	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
+ 	calib_data.AC4 = (I2C_read_ack(I2C1) << 8);
+ 	calib_data.AC4 |= I2C_read_nack(I2C1);
  	I2C_stop(I2C1);
  	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xB2);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
- 	calib_data.AC5 = I2C_read_ack(I2C1);
+	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
+ 	calib_data.AC5 = (I2C_read_ack(I2C1) << 8);
+ 	calib_data.AC5 |= I2C_read_nack(I2C1);
  	I2C_stop(I2C1);
  	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xB4);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
- 	calib_data.AC6 = I2C_read_ack(I2C1);
+	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
+ 	calib_data.AC6 = (I2C_read_ack(I2C1) << 8);
+ 	calib_data.AC6 |= I2C_read_nack(I2C1);
  	I2C_stop(I2C1);
  	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xB6);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
- 	calib_data.B1 = I2C_read_ack(I2C1);
+	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
+ 	calib_data.B1 = (I2C_read_ack(I2C1) << 8);
+ 	calib_data.B1 |= I2C_read_nack(I2C1);
  	I2C_stop(I2C1);
  	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xB8);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
- 	calib_data.B2 = I2C_read_ack(I2C1);
+	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
+ 	calib_data.B2 = (I2C_read_ack(I2C1) << 8);
+ 	calib_data.B2 |= I2C_read_nack(I2C1);
  	I2C_stop(I2C1);
  	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xBA);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
- 	calib_data.MB = I2C_read_ack(I2C1);
+	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
+ 	calib_data.MB = (I2C_read_ack(I2C1) << 8);
+ 	calib_data.MB |= I2C_read_nack(I2C1);
  	I2C_stop(I2C1);
  	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xBC);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
- 	calib_data.MC = I2C_read_ack(I2C1);
+	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
+ 	calib_data.MC = (I2C_read_ack(I2C1) << 8);
+ 	calib_data.MC |= I2C_read_nack(I2C1);
  	I2C_stop(I2C1);
  	I2C_start(I2C1,PRESSURE_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_write(I2C1, 0xBE);
-	I2C_stop(I2C1);
-	I2C_start(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
- 	calib_data.MD = I2C_read_ack(I2C1);
+	I2C_restart(I2C1, PRESSURE_ADDR << 1, I2C_Direction_Receiver);
+ 	calib_data.MD = (I2C_read_ack(I2C1) << 8);
+ 	calib_data.MD |= I2C_read_nack(I2C1);
  	I2C_stop(I2C1);
+
  	printf("PRESSURE: Got callibration data\r\n");
  	return calib_data;
 }
 char* callibration_pressure_data_csv(bmp085_callibration parameters)
 {
 	char *str = chHeapAlloc(NULL,sizeof(char)*(11*11-1));
-	memset(str,0x00,sizeof(char)*(11*11-1));
+	memset(str,0x00,sizeof(char)*(11*11));
 	sprintf(str + strlen(str),"%d,",parameters.AC1);
 	sprintf(str + strlen(str),"%d,",parameters.AC2);
 	sprintf(str + strlen(str),"%d,",parameters.AC3);
@@ -331,8 +309,10 @@ void init_pressure_temperature(void)
 void init_humidity_temp(void)
 {
+	printf("HUMIDITY: Initializing sensor\r\n");
 	I2C_start(I2C1,HUMIDITY_TEMP_ADDR << 1, I2C_Direction_Transmitter);
 	I2C_stop(I2C1);
+	//Reset I2C?
 }
 uint16_t get_humidity_data(void)
 {