diff --git a/app/CMakeLists.txt b/app/CMakeLists.txt
index bc78a3a..8604e2a 100644
--- a/app/CMakeLists.txt
+++ b/app/CMakeLists.txt
@@ -3,7 +3,10 @@ set(elf_file stm32.elf)
 set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/nanopb/extra)
 find_package(Nanopb REQUIRED)
 
-nanopb_generate_cpp(TARGET proto simple.proto)
+nanopb_generate_cpp(TARGET proto 
+    simple.proto
+    quaternion.proto
+)
 
 add_executable(${elf_file}
     app.c    
diff --git a/app/app.c b/app/app.c
index e2e3b39..09345ec 100644
--- a/app/app.c
+++ b/app/app.c
@@ -2,9 +2,29 @@
 #include <pb_encode.h>
 #include <pb_decode.h>
 #include "simple.pb.h"
+#include "quaternion.pb.h"
 
 #define SER_BUF_SIZE 128
-uint8_t ser_buffer[SER_BUF_SIZE];
+uint8_t serdes_buffer[SER_BUF_SIZE];
+
+uint8_t deserialize(uint8_t* buffer, size_t len) {
+    bool status = 0;
+    SimpleMessage message = SimpleMessage_init_zero;
+
+    Quaternion quat = Quaternion_init_zero;
+
+    pb_istream_t stream = pb_istream_from_buffer(buffer, len);
+    
+    status = pb_decode(&stream, Quaternion_fields, &quat);
+    
+    if (!status)
+    {
+        printf("Decoding failed: %s\r\n", PB_GET_ERROR(&stream));
+        return 1;
+    }
+    
+    printf("Quaternion: w: %.2f, x: %.2f, y: %.2f, z: %.2f\r\n", quat.w, quat.x, quat.y, quat.z);
+}
 
 uint8_t serialize(uint8_t* buffer, size_t* len) {
     size_t message_length = 0;
@@ -28,10 +48,12 @@ uint8_t serialize(uint8_t* buffer, size_t* len) {
     return 0;
 }
 
+void commandHandler() {
+
+}
+
 void sendData(sd_callback_t send_data_callback) {
-    if (get_dbg_uart() == NULL) 
-        return;
     size_t ser_len = 0;
-    serialize(ser_buffer, &ser_len);
-    send_data_callback(ser_buffer, ser_len);
+    serialize(serdes_buffer, &ser_len);
+    send_data_callback(serdes_buffer, ser_len);
 }
\ No newline at end of file
diff --git a/app/app.h b/app/app.h
index e31abce..fa9312d 100644
--- a/app/app.h
+++ b/app/app.h
@@ -3,6 +3,7 @@
 #include "bsp.h"
 
 typedef void (*sd_callback_t)(const uint8_t*, uint16_t);
-void sendData(sd_callback_t);
 
+void sendData(sd_callback_t);
+uint8_t deserialize(uint8_t* buffer, size_t len);
 #endif
\ No newline at end of file
diff --git a/app/main.c b/app/main.c
index 12b9fbe..3042acd 100644
--- a/app/main.c
+++ b/app/main.c
@@ -11,9 +11,12 @@ int main(void)
   while (1)
   {
     lwipProcess(); 
-    sendData(udp_send_data);
     GPIO_ToggleBits(GPIOB, GPIO_Pin_14);
-    delay(100);
+    if (getLwipStatus()->udp_packet_rdy == PACKET_RDY) {
+      getLwipStatus()->udp_packet_rdy = PACKET_NOT_RDY;
+      udp_receive_buffer_t* udp_buffer = getUdpReceiveBuffer();
+      deserialize(udp_buffer->buf, udp_buffer->len);
+    }
   }
 
 }
diff --git a/app/quaternion.proto b/app/quaternion.proto
new file mode 100644
index 0000000..2592c4a
--- /dev/null
+++ b/app/quaternion.proto
@@ -0,0 +1,8 @@
+syntax = "proto3";
+
+message Quaternion {
+    float w = 1;
+    float x = 2;
+    float y = 3;
+    float z = 4;
+}
diff --git a/bsp/bsp.c b/bsp/bsp.c
index 98161ef..30af0a2 100644
--- a/bsp/bsp.c
+++ b/bsp/bsp.c
@@ -5,7 +5,7 @@
 
 static USART_TypeDef* dbg_uart = NULL;
 
-static lwip_status_t lwip_status = {.link_status = LINK_DOWN};
+static lwip_status_t lwip_status = {.link_status = LINK_DOWN, .udp_packet_rdy = PACKET_NOT_RDY};
 
 USART_TypeDef* get_dbg_uart() {
     return &dbg_uart;
@@ -19,7 +19,9 @@ void register_dbg_uart(USART_TypeDef* uart) {
     dbg_uart = uart;
 }
 
-void send_uart(const uint8_t* data, uint8_t len) {
+void uart_send_data(const uint8_t* data, uint8_t len) {
+    if (get_dbg_uart() == NULL) 
+        return;
     uint8_t i = 0;
     while(i < len) {
         while (USART_GetFlagStatus(dbg_uart, USART_FLAG_TXE) == RESET); 
diff --git a/bsp/bsp.h b/bsp/bsp.h
index 77e896d..332dc0e 100644
--- a/bsp/bsp.h
+++ b/bsp/bsp.h
@@ -13,15 +13,18 @@
 
 typedef enum  {
   LINK_DOWN,
-  LINK_UP 
+  LINK_UP,
+  PACKET_RDY, 
+  PACKET_NOT_RDY 
 } status_t;
 
 typedef struct {
   uint8_t link_status;
+  uint8_t udp_packet_rdy;
 } lwip_status_t;
 
 void boardInit();
 lwip_status_t* getLwipStatus();
 USART_TypeDef* get_dbg_uart();
-void send_uart(const uint8_t*, uint8_t);
+void uart_send_data(const uint8_t*, uint8_t);
 #endif
\ No newline at end of file
diff --git a/bsp/lwip/lwip.c b/bsp/lwip/lwip.c
index 7a0f119..0d45453 100644
--- a/bsp/lwip/lwip.c
+++ b/bsp/lwip/lwip.c
@@ -8,7 +8,6 @@
 #include "lwip/timeouts.h"
 #include "netif/etharp.h"
 
-#define UDP_RX_BUFFER_SIZE 128
 
 void Error_Handler(void);
 
@@ -24,17 +23,22 @@ uint8_t IP_ADDRESS[4];
 uint8_t NETMASK_ADDRESS[4];
 uint8_t GATEWAY_ADDRESS[4];
 
-static char udp_receive_buffer[UDP_RX_BUFFER_SIZE];
+static udp_receive_buffer_t udp_receive_buffer = {.len = 0};
+
+udp_receive_buffer_t* getUdpReceiveBuffer() {
+  return &udp_receive_buffer;
+}
 
 void udp_receive_callback(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port);
 
 void udp_receive_callback(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port) {
-  strncpy(udp_receive_buffer,p->payload,p->len);
-  udp_receive_buffer[p->len]=0;
-  // printf("Test\r\n");
-  printf("%s\r\n", udp_receive_buffer);
+  strncpy(udp_receive_buffer.buf,p->payload,p->len);
+  udp_receive_buffer.len = p->len;
+  udp_receive_buffer.buf[p->len]=0;
+  // printf("udp packet\r\n");
+  // printf("%s\r\n", udp_receive_buffer.buf);
+  getLwipStatus()->udp_packet_rdy = PACKET_RDY;
   pbuf_free(p);
-  GPIO_ToggleBits(GPIOD, GPIO_Pin_0);
 }
 
 void udpServer_init(void)
@@ -52,7 +56,7 @@ void udpServer_init(void)
     if (err == ERR_OK) {
       // printf("UDP bind\r\n");
     }
-    IP4_ADDR(&remote_ip, 192, 168, 1, 111);
+    IP4_ADDR(&remote_ip, 192, 168, 1, 110);
     // err= udp_connect(upcb, &remote_ip, remote_port);
     if (err == ERR_OK) {
       // printf("UDP connect\r\n");
diff --git a/bsp/lwip/lwip.h b/bsp/lwip/lwip.h
index da7f4c3..e4d8527 100644
--- a/bsp/lwip/lwip.h
+++ b/bsp/lwip/lwip.h
@@ -3,9 +3,16 @@
 
 #include "ethernetif.h"
 
+#define UDP_RX_BUFFER_SIZE 128
+
+typedef struct {
+  size_t len;
+  char buf[UDP_RX_BUFFER_SIZE];
+} udp_receive_buffer_t;
+
 void udpServer_init(void);
 void udp_send_data(const char *data, u16_t len);
 void lwipInit(void);
 void lwipProcess(void);
-
+udp_receive_buffer_t* getUdpReceiveBuffer();
 #endif 
diff --git a/bsp/stm32f4xx_it.c b/bsp/stm32f4xx_it.c
index 1e094ca..547f5b1 100644
--- a/bsp/stm32f4xx_it.c
+++ b/bsp/stm32f4xx_it.c
@@ -1,5 +1,6 @@
 #include "stm32f4xx_it.h"
 #include "lwip/ethernetif.h"
+#include "../app/app.h"
 
 static volatile uint32_t sys_tick = 0;
 
@@ -26,13 +27,13 @@ void delay(uint32_t delay) {
 void TIM7_IRQHandler()
 {
     TIM_ClearITPendingBit(TIM7, TIM_IT_Update); 
-    // GPIO_ToggleBits(GPIOB, GPIO_Pin_7); 
     if(getLwipStatus()->link_status == LINK_UP) {
         GPIO_ToggleBits(GPIOB, GPIO_Pin_7); 
     }
     else {
         GPIO_ResetBits(GPIOB, GPIO_Pin_7);
     }
+    // sendData(udp_send_data);
 }
 
 void USART2_IRQHandler(void)