diff --git a/.gdbinit b/.gdbinit
index 90bca3d74a818810eb71148f4dde17a564b239fb..4220eee7531bcd83fb6572eee3d7e243a4171bb6 100644
--- a/.gdbinit
+++ b/.gdbinit
@@ -1,8 +1,5 @@
-target remote :3333
+target remote 192.168.1.4:3333
 
 monitor reset init
-monitor arm semihosting enable
-monitor tpiu config internal /tmp/itm.log uart off 84000000
-monitor itm port 0 on
-load
+load target/thumbv7em-none-eabihf/debug/d7018e_coap
 monitor reset init
diff --git a/.vscode/launch.json b/.vscode/launch.json
new file mode 100644
index 0000000000000000000000000000000000000000..55e1c379068bc3406a0e108f894642887a75b091
--- /dev/null
+++ b/.vscode/launch.json
@@ -0,0 +1,24 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "type": "gdb",
+            "request": "attach",
+            "name": "Debug embedded",
+            "usewsl": true,
+            "gdbpath": "arm-none-eabi-gdb",
+            "executable": "./target/thumbv7em-none-eabihf/debug/d7018e_coap",
+            "target": "192.168.1.4:3333",
+            "remote": true,
+            "autorun": [
+                "monitor reset init",
+                "load",
+                "monitor reset init"
+            ],
+            "cwd": "${workspaceRoot}"
+        },
+    ]
+}
\ No newline at end of file
diff --git a/Cargo.toml b/Cargo.toml
index 5b931df0918e7fe4f62ab9a23b2a507f1d846d51..479081e4197e7ccb11ed8b322582c4046f4fe5b9 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -15,9 +15,10 @@ cortex-m-semihosting = "0.2.0"
 static-ref = "0.2.0"
 volatile-register = "0.2.0"
 m = "0.1.1"
-heapless = "0.2.1"
+heapless = "0.2.4"
 f4 = {git = "https://github.com/jsjolund/f4"}
-xoap = { path = "../projects/xoap", version = "0.1.0", default-features = false, features = ["baremetal"] }#git = "https://github.com/Shawnshank/xoap", version = "0.1.0"}
+stm32f40x = "0.2.0"
+#xoap = { path = "../projects/xoap", version = "0.1.0", default-features = false, features = ["baremetal"] }# git = "https://github.com/Shawnshank/xoap"
 
 [dependencies.cast]
 default-features = false
diff --git a/memory.x b/memory.x
index 8462e02743c730756c84bbc9c0d8a54abb30c051..d27050f821a3f9abe8c4166837a8d552918f2bfc 100644
--- a/memory.x
+++ b/memory.x
@@ -2,8 +2,8 @@ MEMORY
 {
   /* NOTE K = KiBi = 1024 bytes */
   /* TODO Adjust these memory regions to match your device memory layout */
-  FLASH : ORIGIN =  0x08000000, LENGTH = 512K
-  RAM : ORIGIN = 0x20000000, LENGTH = 96K
+  FLASH : ORIGIN =  0x08000000, LENGTH = 511K
+  RAM : ORIGIN = 0x20000000, LENGTH = 128K
 }
 
 /* This is where the call stack will be allocated. */
diff --git a/src/main.rs b/src/main.rs
index 1b3acf480694e15d50c8778a812b9c2fca9050bc..f61c50916173d6eadf55fbc4631ffb64ef1bae5e 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,17 +1,19 @@
-//! Prints "Hello" and then "World" in the OpenOCD console
+//! Test sending messages and receiving commands using the DMA
 #![deny(unsafe_code)]
 #![deny(warnings)]
+#![allow(dead_code)]
+#![allow(non_snake_case)]
 #![feature(const_fn)]
 #![feature(proc_macro)]
 #![feature(lang_items)]
 #![no_std]
 
+extern crate cortex_m;
 extern crate cortex_m_rtfm as rtfm;
-//extern crate cortex_m_semihosting as semihosting;
 #[macro_use]
 extern crate f4;
 extern crate heapless;
-extern crate xoap;
+extern crate stm32f40x;
 
 use core::fmt::Write;
 use core::ops::Deref;
@@ -20,22 +22,38 @@ use f4::U8Writer;
 use f4::prelude::*;
 use f4::dma::{Buffer, Dma1Stream5, Dma1Stream6};
 use f4::time::Hertz;
+//use f4::Timer;
 use f4::clock;
+use f4::I2c;
 use heapless::Vec;
 use rtfm::{app, Threshold};
-//use semihosting::hio::{self, HStdout};
+//use core::result::Result;
+//use f4::stm32f40x::I2C1;
 
 // Max length of a command to be parsed.
 const MAX_CMD_LEN: usize = 10;
 // Max length of an output string that can be sent.
 const MAX_TX_LEN: usize = 100;
 // Max length of input buffer. Since we are parsing for commands, we need to check each received character.
-const MAX_RX_LEN: usize = 100;
-
-const BAUD_RATE: Hertz = Hertz(115_200);
+const MAX_RX_LEN: usize = 1;
+// Baud rate of the serial interface
+const BAUD_RATE: Hertz = Hertz(9_600);
+// Temperature and humidity update frequency
+const SENSOR_READ_TIME: Hertz = Hertz(1);
+//const DATA_RECIEVE_SIZE: usize = 4;
+const RX_BUFFER_SIZE: usize = core::mem::size_of::<u32>();
 
+// Errors from the HIH6030-021-001
+#[derive(Debug)]
+pub enum Error {
+    StaleData,
+    CommandMode,
+    Unknown,
+}
+// Command types from the serial interface
 enum CmdType {
     Greeting,
+    Data,
     Unknown,
     None,
 }
@@ -48,22 +66,62 @@ app! {
         static RX_BUFFER: Buffer<[u8; MAX_RX_LEN], Dma1Stream5> = Buffer::new([0; MAX_RX_LEN]);
         static TX_BUFFER: Buffer<[u8; MAX_TX_LEN], Dma1Stream6> = Buffer::new([0; MAX_TX_LEN]);
         static CNT: u8 = 0;
+        // static CMD: CmdType = CmdType::None;
+        static TEMPERATURE: f32 = 0.0;
+        static HUMIDITY: f32 = 0.0;
     },
 
     tasks: {
         DMA1_STREAM5: {
             path: rx_done,
             priority: 1,
-            resources: [CMD_BUFFER, RX_BUFFER, TX_BUFFER, DMA1, USART2, CNT],
+            resources: [CMD_BUFFER, RX_BUFFER, TX_BUFFER, DMA1, USART2, CNT, TEMPERATURE, HUMIDITY],   // include CMD
         },
         DMA1_STREAM6: {
             path: tx_done,
             priority: 2,
             resources: [TX_BUFFER, DMA1],
         },
+        /*TIM2: {
+            path: read_sensor,
+            priority: 3,
+            resources: [TIM2, I2C1, TEMPERATURE, HUMIDITY],
+        },*/
     },
 }
 
+fn init(p: init::Peripherals, r: init::Resources) {
+    // Set clock to higher than default in order to test that it works
+    clock::set_84_mhz(&p.RCC, &p.FLASH);
+
+    // Start the serial port
+    let serial = Serial(p.USART2);
+    serial.init(BAUD_RATE.invert(), Some(p.DMA1), p.GPIOA, p.RCC);
+
+    // Start the I2C peripheral
+    let i2c = I2c(p.I2C1);
+    i2c.init(p.GPIOA, p.GPIOB, p.RCC);
+    i2c.enable();
+
+    // Start the timer for the update of the Temp/RH sensor.
+    /*let timer = Timer(&*p.TIM2);
+    timer.init(SENSOR_READ_TIME.invert(), p.RCC);
+    timer.resume();
+    */
+    // FIXME: We cannot use the uprint macro in the init since it needs Resources
+    // and Threshold...
+    // Send a welcome message by borrowing a slice of the transmit buffer and
+    // writing a formatted string into it.
+    write!(
+        U8Writer::new(&mut r.TX_BUFFER.borrow_mut()[..MAX_TX_LEN]),
+        "Hello, world! Say hi to me!\r\n",
+    ).unwrap();
+    serial.write_all(p.DMA1, r.TX_BUFFER).unwrap();
+
+    // Listen to serial input on the receive DMA
+    serial.read_exact(p.DMA1, r.RX_BUFFER).unwrap();
+}
+
 // Interrupt for serial receive DMA
 fn rx_done(t: &mut Threshold, mut r: DMA1_STREAM5::Resources) {
     use rtfm::Resource;
@@ -81,7 +139,7 @@ fn rx_done(t: &mut Threshold, mut r: DMA1_STREAM5::Resources) {
         r.USART2.claim(t, |usart, t| {
             let serial = Serial(&**usart);
             if serial.write(byte).is_err() {
-                rtfm::bkpt();
+                //rtfm::bkpt();
             }
             if byte == b'\r' {
                 // If we got carrige return, send new line as well
@@ -101,6 +159,7 @@ fn rx_done(t: &mut Threshold, mut r: DMA1_STREAM5::Resources) {
             // End of command
             cmd_type = match &***cmd {
                 b"hi" | b"Hi" => CmdType::Greeting,
+                b"data" => CmdType::Data,
                 _ => {
                     if cmd.len() == 0 {
                         CmdType::None // Empty string
@@ -127,6 +186,11 @@ fn rx_done(t: &mut Threshold, mut r: DMA1_STREAM5::Resources) {
             // Print a response using DMA
             uprint!(t, r.USART2, r.DMA1, r.TX_BUFFER, "Hi counter {}!\r\n", cnt);
         }
+        
+        CmdType::Data => {
+            //let test: u8 = **r.TEMPERATURE;
+            uprint!(t, r.USART2, r.DMA1, r.TX_BUFFER, "Temp:!\r\n");
+        }
         CmdType::Unknown => {
             // Unknown command
             uprint!(t, r.USART2, r.DMA1, r.TX_BUFFER, "That's no greeting.\r\n");
@@ -149,27 +213,64 @@ fn tx_done(t: &mut Threshold, r: DMA1_STREAM6::Resources) {
     });
 }
 
-fn init(_p: init::Peripherals, r: init::Resources) {
-    // Set clock to higher than default in order to test that it works
-    clock::set_84_mhz(&_p.RCC, &_p.FLASH);
+/*
+// Interrupt for the timer for the update of the Temp/RH sensor
+fn read_sensor(_t: &mut Threshold, r: TIM2::Resources) {
+    // Clear the interrupt flag
+    r.TIM2.sr.modify(|_, w| w.uif().clear_bit());
+    // I2C interface.
+    let i2c = I2c(&**r.I2C1);
+    // Read back to check that it worked
+    let mut rx: [u8; RX_BUFFER_SIZE] = [0; RX_BUFFER_SIZE];
+    match get_data(&i2c, &mut rx) {
+        Err(_) => {}
+        Ok(_) => {
+            // Calculate temperature and store the value in the resources
+            let mut _temp: f32 = (((rx[2] << 6) | (rx[3] >> 2)) as f32) * 100.0 / 16382.0;
+            //let mut _temp: f32 = (_T as f32) * 100.0 / 16382.0;
+            **r.TEMPERATURE = _temp;
+            // Calculate humidity and store the value in the resources
+            let mut _RH: f32 = (((rx[2] << 6) | (rx[3] >> 2)) as f32) * 165.0 / 16382.0 - 40.0;
+            **r.HUMIDITY = _RH;
+        }
+    }
+}
 
-    // Start the serial port
-    let serial = Serial(_p.USART2);
-    serial.init(BAUD_RATE.invert(), Some(_p.DMA1), _p.GPIOA, _p.RCC);
+// Gets the data from the Temp/RH sensor over the I2C bus
+fn get_data(i2c: &I2c<I2C1>, rx_buffer: &mut [u8; RX_BUFFER_SIZE]) -> Result<(), Error> {
+    while i2c.start(0x4E).is_err() {} // 0x4E - write (2*27), 0x4F - Read (2*27)+1
+                                      //while i2c.write(0).is_err() {}
+    while i2c.stop().is_err() {}
 
-    // FIXME: We cannot use the uprint macro in the init since it needs Resources
-    // and Threshold...
-    // Send a welcome message by borrowing a slice of the transmit buffer and
-    // writing a formatted string into it.
-    write!(
-        U8Writer::new(&mut r.TX_BUFFER.borrow_mut()[..MAX_TX_LEN]),
-        "Hello, world! Say hi to me!\r\n",
-    ).unwrap();
-    serial.write_all(_p.DMA1, r.TX_BUFFER).unwrap();
+    // Read incoming bytes and ACK them
+    while i2c.start(0x4F).is_err() {}
+    for i in 0..RX_BUFFER_SIZE {
+        rx_buffer[i] = loop {
+            if i == RX_BUFFER_SIZE - 1 {
+                // Do not ACK the last byte received and send STOP
+                if let Ok(byte) = i2c.read_nack() {
+                    break byte;
+                }
+            } else {
+                // ACK the byte after receiving
+                if let Ok(byte) = i2c.read_ack() {
+                    break byte;
+                }
+            }
+        }
+    }
+    while i2c.stop().is_err() {}
 
-    // Listen to serial input on the receive DMA
-    serial.read_exact(_p.DMA1, r.RX_BUFFER).unwrap();
+    let status: u8 = (rx_buffer[0] & 0xC0) >> 6;
+    match status {
+        0 => Ok(()),
+        1 => Err(Error::StaleData),
+        2 => Err(Error::CommandMode),
+        3 => Err(Error::Unknown),
+        _ => Err(Error::Unknown),
+    }
 }
+*/
 
 fn idle() -> ! {
     loop {