diff --git a/src/main.c b/src/main.c
index 80fe00f..7c6f129 100644
--- a/src/main.c
+++ b/src/main.c
@@ -9,6 +9,7 @@
 #include <util/delay.h>
 #include <util/setbaud.h>
 
+#include "sensor_bme280.h"
 #include "bme280.h"
 #include "i2c.h"
 #include <stdio.h>
@@ -41,6 +42,35 @@ void uart_putstr(const char *str) {
     }
 }
 
+char * format_temperature(float temp) {
+    uint8_t min_width = 3;
+
+    char * out[min_width];
+    dtostrf(temp, min_width, 1, out);
+
+    return out;
+}
+
+char * format_humidity(float humidity) {
+    uint8_t min_width = 5;
+
+    char out[min_width];
+    dtostrf(humidity, min_width, 2, out);
+
+    return out;
+}
+
+char * format_pressure(float pressure) {
+    uint8_t min_width = 3;
+
+    char out[min_width];
+    dtostrf(pressure, min_width, 1, out);
+
+    return out;
+}
+
+
+
 int main(void)
 {
     uart_init();
@@ -52,29 +82,44 @@ int main(void)
     float pressure = 0.0;
     float humidity = 0.0;
 
-    _delay_ms(500);
     i2c_init();
-    i2c_start((0x76 << 1) | 0x00);
-    i2c_byte(0xD0);
-    i2c_stop();
+    _delay_ms(500);
 
-    _delay_us(10);
-    i2c_start((0x76 << 1) | 0x01);
-    uint8_t ret = i2c_readAck();
-    i2c_stop();
-
-    if(ret == 0x60) { uart_putstr("\nbme280 detected.\n"); }
-    //temp = bme280_readTemperature(0);
-    //pressure = bme280_readPressure(0);
-    //humidity = bme280_readHumidity(0);
+    uint8_t ret = bme280_init(1);
+    if(ret == 0x00) {
+        uart_putstr("\nbme280 detected by library.\r\n");
+    } else if(ret == 0x01) {
+        uart_putstr("\nbmp280 detected by library.\r\n");
+    } else if(ret == 0xFF) {
+        uart_putstr("\nno sensor detected.\r\n");
+    } else {
+        uart_putstr("\nunknown error.\r\n");
+    }
 
+    // Turn the green led on
+    PORTB |= 1 << 5;
 
     while (1)
     {
         _delay_ms(1000);
 
-        // toggle the LED
-        PORTB ^= 1 << 5;
+        temp = bme280_readTemperature(1);
+        pressure = bme280_readPressure(1);
+        humidity = bme280_readHumidity(1);
+
+        char out[10];
+        dtostrf(temp, 3, 1, out);
+        uart_putstr("T");
+        uart_putstr(out);
+        uart_putstr("\r\n");
+        dtostrf(pressure, 3, 1, out);
+        uart_putstr("P");
+        uart_putstr(out);
+        uart_putstr("\r\n");
+        dtostrf(humidity, 3, 1, out);
+        uart_putstr("H");
+        uart_putstr(out);
+        uart_putstr("\r\n");
     }
 
     return 0;
diff --git a/src/sensor_bme280.c b/src/sensor_bme280.c
new file mode 100644
index 0000000..470d318
--- /dev/null
+++ b/src/sensor_bme280.c
@@ -0,0 +1,33 @@
+#include "sensor_bme280.h"
+#include "bme280.h"
+#include "i2c.h"
+
+#include <util/delay.h>
+
+/**
+ * Checks whether the bme280 is connected to the i2c bus
+ * Returns 0 if success, 1 if error
+ */
+sensor_status check_sensor(void) {
+    i2c_init();
+    i2c_start((0x76 << 1) | 0x00);
+    i2c_byte(0xD0);
+    i2c_stop();
+
+    _delay_us(10);
+    i2c_start((0x76 << 1) | 0x01);
+    uint8_t ret = i2c_readNAck();
+    i2c_stop();
+
+    _delay_us(100);
+    i2c_start((0x76 << 1) | 0x00);
+    i2c_byte(BME280_REGISTER_SOFTRESET);
+    i2c_byte(0xB6);
+    i2c_stop();
+
+    if(ret == 0x60) {
+        return SENSOR_OK;
+    } else {
+        return SENSOR_NOT_FOUND;
+    }
+}
diff --git a/src/sensor_bme280.h b/src/sensor_bme280.h
new file mode 100644
index 0000000..c6e6651
--- /dev/null
+++ b/src/sensor_bme280.h
@@ -0,0 +1,8 @@
+#pragma once
+
+typedef enum {
+    SENSOR_OK,
+    SENSOR_NOT_FOUND,
+} sensor_status;
+
+sensor_status check_sensor(void);