Optimizations for master data and command routing handling and corresponding changes for slave data and command decoding. Master no longer decodes data in order send individual datapoints in a packed data_message field individually, but stores and forwards the encoded data message and MDR to the slaves during routing. This significantly optimizes data routing by reducing the time taken to decode and encode every single packed datapoint.

Task List: None

5 files changed, 136 insertions, 92 deletions

diff --git a/makefile b/makefile
index c71087a..22224c0 100644
--- a/makefile
+++ b/makefile
@@ -1,5 +1,5 @@
 # Target also determines which file is being compiled

-TARGET = slave-cmd

+TARGET = master

 

 

 ######################################

diff --git a/src/config.h b/src/config.h
index ad035c9..e4a6590 100644
--- a/src/config.h
+++ b/src/config.h
@@ -1,9 +1,9 @@
 
 /* Enable debug mode */
-#define DEBUG_ENABLE 1
+/* #define DEBUG_ENABLE 1 */
 
 /* Enable testing mode */
-#define TESTING_ENABLE 1
+/* #define TESTING_ENABLE 1 */
 
 /* Buffer size for routing */
 #define ROUTING_BUFSIZE 128
@@ -19,4 +19,4 @@
  * hardware.
  *
 */
-#define MASTER_I2C_BUS_INTERVAL 150
+#define MASTER_I2C_BUS_INTERVAL 5
diff --git a/src/master.c b/src/master.c
index 4cad251..28ff4bb 100644
--- a/src/master.c
+++ b/src/master.c
@@ -40,7 +40,9 @@
 
 /* Private globals */
 I2C_HandleTypeDef hi2c1;
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
 UART_HandleTypeDef huart1;
+#endif
 
 device_info_t *device_info[BUS_DEVICE_LIMIT] = {NULL};
 subscription_info_t* subs_info[BUS_DEVICE_LIMIT];
@@ -48,21 +50,26 @@ uint32_t allocated[4]={0};
 uint8_t dev_sts[BUS_DEVICE_LIMIT] = {OFFLINE};
 uint8_t data_idx;
 
-_datapoint routing_buffer[ROUTING_BUFSIZE];   /*< Buffer to store data to be routed */
-uint8_t    *cmd_routing_buf[ROUTING_BUFSIZE]; /*< Buffer to store commands to be routed */
+uint8_t *data_rbuf[ROUTING_BUFSIZE];        /*< Buffer to store data to be routed */
+uint8_t *cmd_routing_buf[ROUTING_BUFSIZE];  /*< Buffer to store commands to be routed */
 
-uint8_t    routing_idx_buffer[ROUTING_BUFSIZE];  /*< Index information for data source */
+uint8_t    data_src_idx_rbuf[ROUTING_BUFSIZE];   /*< Index information for data source */
 uint8_t    cmd_src_idx_rbuf[ROUTING_BUFSIZE];    /*< Index information for command source */
 uint8_t    cmd_dst_idx_rbuf[ROUTING_BUFSIZE];    /*< Index information for command dest */
 
-uint32_t   routing_ptr     = 0; /*< Pointer to tail of both data and data index buffers */
-uint32_t   cmd_routing_ptr = 0; /*< Pointer to tail of cmd and cmd index buffers */
+uint32_t    data_len_buf[ROUTING_BUFSIZE];
+
+uint32_t   data_routing_ptr = 0;     /*< Pointer to tail of both data and data index buffers */
+uint32_t   cmd_routing_ptr = 0;      /*< Pointer to tail of cmd and cmd index buffers */
     
 /* Function prototypes */
 void SystemClock_Config(void);
 static void MX_GPIO_Init(void);
 static void MX_I2C1_Init(void);
+
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
 static void MX_USART1_UART_Init(void);
+#endif
 
 hs_status_t handshake(uint32_t i2c_addr);
 dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t routing_buf_idx);
@@ -94,8 +101,11 @@ int main(void)
     /* Initialize all configured peripherals */
     MX_GPIO_Init();
     MX_I2C1_Init();
-    MX_USART1_UART_Init();
 
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+    MX_USART1_UART_Init();
+#endif
+    
 #ifdef TESTING_ENABLE
 #ifdef MASTER
     uint8_t reset_string[] = "\r\n\n==========MASTER RESET=========\r\n\n";
@@ -106,7 +116,7 @@ int main(void)
 #endif /* MASTER */
 #endif /* TESTING_ENABLE */
     
-    uint8_t priority_counter = 0, debug_buf[128] = {0};
+    uint8_t priority_counter = 0;
     /* Handshake */
     while (1) {	
 	if (priority_counter == 0) {
@@ -293,8 +303,8 @@ hs_status_t handshake(uint32_t i2c_addr)
 	memset(debug_buf, 0, 128);
 	goto __HS_MDR_MDR_TESTING_END;
     __MDR_DEC_TESTING:
-	sprintf((char*)debug_buf, "MDR Decode success\r\n\tFirst subscibed module: %d\r\n",
-		subs_info[dev_idx]->module_ids[1]);
+	sprintf((char*)debug_buf, "MDR Decode success\r\n\tFirst subscibed module: %x\r\n",
+		subs_info[dev_idx]->module_ids[0]);
 	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	memset(debug_buf, 0, 128);
 	goto __MDR_DEC_TESTING_END;
@@ -339,22 +349,19 @@ __TESTING_BLOCK_END:
     return hs_sts;
 }
 
-dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t rbuf_idx)
+dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, volatile uint8_t rbuf_idx)
 {
     uint8_t dev_idx = GET_IDX_FROM_ADDR(i2c_addr);    
     dataflow_status_t df_status = DF_IDLE;
     uint8_t CTS_buf[2] = {0x2, 0xFF};
     
     uint8_t DOC_buf[4];
-    uint8_t *data_buf;
     uint8_t cmd_dest;
     
     uint32_t AF_error_counter = 0;
     uint32_t data_len         = 0;
-    _datapoint datapoints[16];
     
     /* TODO Add default values to the CTS message in proto */
-    s2m_data data_message = s2m_data_init_zero;
 #if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
     uint8_t debug_buf[128]={0};
 #endif
@@ -409,7 +416,11 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		}
 	    }
 	    if (df_status != DF_FAIL) {
-		if (DOC_buf[1] == DATA) {
+		if (DOC_buf[0] == 0x0) {
+		    /* Do nothing DOC; should become redundant once dataflow is initiated using classes */
+		    df_status = DF_SUCCESS;
+		}
+		else if (DOC_buf[1] == DATA) {
 		    df_status = DF_CTS;
 		    data_len = DOC_buf[3];
 		}
@@ -433,7 +444,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	case (DF_CTS):
 	{
 	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
-	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 10000) != HAL_OK) {
+	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 1000) != HAL_OK) {
 		df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
 		goto __DF_CTS_I2C_ERROR;
@@ -455,15 +466,12 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	}
 	case (DF_RX_DATA):
 	{
-	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);
-	    sprintf((char*)debug_buf, "data len: %ld\r\n", data_len);
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
-	    
-	    data_buf = (uint8_t*)malloc(128);
+	    HAL_Delay(MASTER_I2C_BUS_INTERVAL);	    
+	    data_rbuf[data_routing_ptr] = malloc(sizeof(uint8_t)*data_len);
 	    AF_error_counter = 0;
 	    while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr,
-					  (uint8_t*)data_buf, data_len, 1000) != HAL_OK) {
+					  (uint8_t*)data_rbuf[data_routing_ptr],
+					  data_len, 1000) != HAL_OK) {
 		if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
 		    df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
@@ -478,27 +486,14 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		    break;
 		}
 	    }
-	    if (df_status != DF_FAIL) {
-		data_idx = 0;
-		data_message.datapoints.funcs.decode = decode_data_callback;
-		data_message.datapoints.arg = (void*)datapoints;
-		pb_istream_t data_istream = pb_istream_from_buffer(data_buf, data_len);
-		if (!pb_decode(&data_istream, s2m_data_fields, &data_message)) {
-		    df_status = DF_FAIL;
-#ifdef DEBUG_ENABLE
-		    goto __DF_DATA_DECODE_ERROR;
-		__DF_DATA_DECODE_ERROR_END:
-		    __asm__("nop");
-#endif
-		}
-		else {
-		    /* This could be done in the callback itself */
-		    for (int i = 0; i < data_idx && routing_ptr < ROUTING_BUFSIZE; i++) {
-			routing_idx_buffer[routing_ptr] = dev_idx;
-			routing_buffer[routing_ptr++]   = datapoints[i];
-		    }
-		    df_status = DF_SUCCESS;
-		}
+	    if (df_status != DF_FAIL) {		
+		data_src_idx_rbuf[data_routing_ptr] = dev_idx;
+		data_len_buf[data_routing_ptr] = (uint8_t)data_len;
+		data_routing_ptr++;
+		df_status = DF_SUCCESS;
+	    }
+	    else {
+		free(data_rbuf[data_routing_ptr]);
 	    }
 	    break;
 	}
@@ -510,8 +505,8 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	       Once cached, will not need to do this */
 
 	    /* Do this after handshake to cache ==================================== */
-	    uint8_t MDR_buf[128], data_buf[128], CTS_buf[2];
-	    uint8_t src_device_idx = routing_idx_buffer[rbuf_idx];
+	    uint8_t MDR_buf[128];
+	    uint8_t src_device_idx = data_src_idx_rbuf[rbuf_idx];
 	    s2m_MDR_response data_src_MDR = device_info[src_device_idx]->MDR;
 	    pb_ostream_t MDR_ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf));
 	    data_src_MDR.subscriptions.funcs.encode=master_encode_MDR_callback;
@@ -519,11 +514,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	    uint8_t MDR_len = MDR_ostream.bytes_written;
 	    /* ==================================================================== */
 	    
-	    _datapoint data = routing_buffer[rbuf_idx];
-	    pb_ostream_t data_ostream = pb_ostream_from_buffer(data_buf, sizeof(data_buf));
-	    pb_encode(&data_ostream, _datapoint_fields, &data);
-	    uint8_t data_len = data_ostream.bytes_written;
-
+	    uint8_t data_len = data_len_buf[rbuf_idx];
 	    uint8_t data_MDR_len_buf[4] = {0, MDR_len, 0, data_len};
 
 	    uint8_t status = get_CTS(i2c_addr);
@@ -543,20 +534,19 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
 #endif
-	    }
-	    
+	    }	    
 	    status = get_CTS(i2c_addr);
 	    if (status != 0 &&
 		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, MDR_buf,
 					MDR_len, 10000) == HAL_OK &&
-		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, data_buf,
+		HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)i2c_addr, data_rbuf[rbuf_idx],
 					data_len, 10000) == HAL_OK) {
+		df_status = DF_SUCCESS;
 #ifdef DEBUG_ENABLE
 		sprintf((char*)debug_buf, "Data and MDR sent\r\n");
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
 #endif
-		df_status = DF_SUCCESS;
 	    }
 	    else {
 		df_status = DF_FAIL;
@@ -617,7 +607,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 					4, 10000) != HAL_OK) {
 		df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
-		sprintf((char*)debug_buf, "Failed to send cmd len buf to %ld\r\n", i2c_addr>>1);
+		sprintf((char*)debug_buf, "Failed to send cmd len buf to %d\r\n", i2c_addr>>1);
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
 #endif
@@ -631,7 +621,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 					    4, 10000) == HAL_OK) {
 		    df_status = DF_SUCCESS;
 #ifdef DEBUG_ENABLE
-		    sprintf((char*)debug_buf, "Routed cmd to %ld\r\n", i2c_addr>>1);
+		    sprintf((char*)debug_buf, "Routed cmd to %d\r\n", i2c_addr>>1);
 		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		    memset(debug_buf, 0, 128);
 #endif
@@ -639,7 +629,7 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 		else {
 		    df_status = DF_FAIL;
 #ifdef DEBUG_ENABLE
-		    sprintf((char*)debug_buf, "Failed to send cmd to %ld\r\n", i2c_addr>>1);
+		    sprintf((char*)debug_buf, "Failed to send cmd to %d\r\n", i2c_addr>>1);
 		    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		    memset(debug_buf, 0, 128);
 #endif
@@ -685,11 +675,6 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	memset(debug_buf, 0, 128);
 	goto __DF_DATA_I2C_ERROR_END;
-    __DF_DATA_DECODE_ERROR:
-	sprintf((char*)debug_buf, "Data decoding error\r\n");
-	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	memset(debug_buf, 0, 128);
-	goto __DF_DATA_DECODE_ERROR_END;
     __DF_DEBUG_BLOCK_END:
 	__asm__("nop");
     }
@@ -700,7 +685,12 @@ dataflow_status_t device_dataflow(uint8_t i2c_addr, uint32_t SOR_code, uint8_t r
 
 uint8_t get_CTS(uint8_t i2c_addr)
 {
-    uint8_t CTS_buf[2], AF_error_counter = 0, debug_buf[20] = {0};
+    uint8_t CTS_buf[2], AF_error_counter = 0;
+
+    #if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
+	debug_buf[20] = {0};
+    #endif
+	
     uint8_t status = 1;
     while (HAL_I2C_Master_Receive(&hi2c1, (uint16_t)i2c_addr, CTS_buf, 2, 10000) != HAL_OK) {
 	if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {
@@ -730,8 +720,8 @@ bool routing(void)
     uint32_t routing_table[ROUTING_BUFSIZE][4] = {{0, 0}};
     
     /* Build table with routing information */
-    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < routing_ptr; rbuf_data_idx++) {
-	uint8_t src_module_idx = routing_idx_buffer[rbuf_data_idx];
+    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < data_routing_ptr; rbuf_data_idx++) {
+	uint8_t src_module_idx = data_src_idx_rbuf[rbuf_data_idx];
 	for (uint8_t dev_idx = 0; dev_idx < BUS_DEVICE_LIMIT; dev_idx++) {
 	    if (!(GET_BIT_FROM_IDX(allocated, dev_idx) && 1)) { // No module at this index
 		continue;
@@ -749,17 +739,18 @@ bool routing(void)
 	}
     }
 
-    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < routing_ptr; rbuf_data_idx++) {
+    for (uint8_t rbuf_data_idx = 0; rbuf_data_idx < data_routing_ptr; rbuf_data_idx++) {
 	for (uint8_t device_idx = 0; device_idx < BUS_DEVICE_LIMIT; device_idx++) {
 	    if (GET_BIT_FROM_IDX(allocated, device_idx) &&
 		GET_BIT_FROM_IDX(routing_table[rbuf_data_idx], device_idx)) {
 		device_dataflow(GET_ADDR_FROM_IDX(device_idx), SLAVE_RX_DATAPOINT, rbuf_data_idx);
 	    }
 	}
+	free(data_rbuf[rbuf_data_idx]);
     }
 
     /* Reset the routing pointer, since all data in buffer should have been routed */    
-    routing_ptr = 0;
+    data_routing_ptr = 0;
     return true;
 }
 
@@ -779,6 +770,7 @@ bool cmd_routing(void)
 	    	device_dataflow(GET_ADDR_FROM_IDX(dev_idx), SLAVE_RX_COMMAND, rbuf_cmd_idx);
 	    }
 	}
+	free(cmd_routing_buf[rbuf_cmd_idx]);
     }
     cmd_routing_ptr = 0;
     return true;
@@ -985,7 +977,7 @@ void SystemClock_Config(void)
 static void MX_I2C1_Init(void)
 {
     hi2c1.Instance = I2C1;
-    hi2c1.Init.ClockSpeed = 100000;
+    hi2c1.Init.ClockSpeed = 400000;
     hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
     hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
     hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
@@ -1005,6 +997,7 @@ static void MX_I2C1_Init(void)
  * @param None
  * @retval None
  */
+#if defined(TESTING_ENABLE) || defined(DEBUG_ENABLE)
 static void MX_USART1_UART_Init(void)
 {
     huart1.Instance = USART2;
@@ -1021,7 +1014,7 @@ static void MX_USART1_UART_Init(void)
     }
 
 }
-
+#endif
 /**
  * @brief GPIO Initialization Function
  * @param None
diff --git a/src/slave-cmd.c b/src/slave-cmd.c
index 5a7153e..c8425fb 100644
--- a/src/slave-cmd.c
+++ b/src/slave-cmd.c
@@ -160,7 +160,7 @@ int main(void)
 	else if (SOR_buf[0] == 2) {
 	    uint8_t CTS_buf[] = {0x0, 0x1};
 	    uint8_t len_buf[4], *MDR_buf, *data_buf;
-	    /* _datapoint datapoints[16]; */
+	    _datapoint datapoints[16];
 	    
 	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
 	    sprintf((char*)debug_buf, "Sent CTS\r\n");
@@ -181,18 +181,27 @@ int main(void)
 	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
 	    HAL_I2C_Slave_Receive(&hi2c1, MDR_buf, MDR_len, 10000);
 	    HAL_I2C_Slave_Receive(&hi2c1, data_buf, data_len, 10000);
-		
-	    _datapoint datapoint_message;
+
 	    s2m_MDR_response MDR_message;
+	    s2m_data data_message;
+	    data_idx = 0;
+	    data_message.datapoints.funcs.decode = decode_data_callback;
+	    data_message.datapoints.arg = (void*)datapoints;
+	    
 	    pb_istream_t MDR_istream  = pb_istream_from_buffer(MDR_buf, MDR_len);
-	    pb_istream_t data_istream = pb_istream_from_buffer(data_buf, data_len);
+	    pb_istream_t data_istream = pb_istream_from_buffer(data_buf, data_len);	    
 	    
 	    pb_decode(&MDR_istream, s2m_MDR_response_fields, &MDR_message);
-	    pb_decode(&data_istream, _datapoint_fields, &datapoint_message);
-
-	    sprintf((char*)debug_buf, "Got data from %ld, running version %f\r\n\tdata 0: %f\r\n", MDR_message.module_id, MDR_message.MDR_version, datapoint_message.data);
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
+	    if (!pb_decode(&data_istream, s2m_data_fields, &data_message)){
+		sprintf((char*)debug_buf, "Data decode error\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Got data from %ld\r\n\tEntity: %ld, data : %f, unit: %ld\r\n", MDR_message.module_id, datapoints[0].entity_id, datapoints[0].data, datapoints[0].unit_id);
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
 	}
 	else if (SOR_buf[0] == 3) {
 	    uint8_t CTS_buf[] = {0x0, 0x1}, len_buf[4], *cmd_buf;
diff --git a/src/slave.c b/src/slave.c
index 7bcceac..f5096f3 100644
--- a/src/slave.c
+++ b/src/slave.c
@@ -43,6 +43,7 @@ UART_HandleTypeDef huart1;
 uint8_t cmd_str[128];
 uint8_t str_ptr = 0;
 pb_byte_t str_buf[18];
+uint8_t data_idx = 0;
 
 /* Function prototypes */
 void SystemClock_Config(void);
@@ -150,7 +151,7 @@ int main(void)
 	else if (SOR_buf[0] == 2) {
 	    uint8_t CTS_buf[] = {0x0, 0x1};
 	    uint8_t len_buf[4], *MDR_buf, *data_buf;
-	    /* _datapoint datapoints[16]; */
+	    _datapoint datapoints[16];
 	    
 	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
 	    sprintf((char*)debug_buf, "Sent CTS\r\n");
@@ -171,18 +172,27 @@ int main(void)
 	    HAL_I2C_Slave_Transmit(&hi2c1, CTS_buf, 2, 10000);
 	    HAL_I2C_Slave_Receive(&hi2c1, MDR_buf, MDR_len, 10000);
 	    HAL_I2C_Slave_Receive(&hi2c1, data_buf, data_len, 10000);
-		
-	    _datapoint datapoint_message;
+
 	    s2m_MDR_response MDR_message;
+	    s2m_data data_message;
+	    data_idx = 0;
+	    data_message.datapoints.funcs.decode = decode_data_callback;
+	    data_message.datapoints.arg = (void*)datapoints;
+	    
 	    pb_istream_t MDR_istream  = pb_istream_from_buffer(MDR_buf, MDR_len);
 	    pb_istream_t data_istream = pb_istream_from_buffer(data_buf, data_len);
-	    
+	    	    
 	    pb_decode(&MDR_istream, s2m_MDR_response_fields, &MDR_message);
-	    pb_decode(&data_istream, _datapoint_fields, &datapoint_message);
-
-	    sprintf((char*)debug_buf, "Got data from %ld, running version %f\r\n\tdata 0: %f\r\n", MDR_message.module_id, MDR_message.MDR_version, datapoint_message.data);
-	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
+	    if (!pb_decode(&data_istream, s2m_data_fields, &data_message)){
+		sprintf((char*)debug_buf, "Data decode error\r\n");
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
+	    else {
+		sprintf((char*)debug_buf, "Got data from %ld\r\n\tEntity: %ld, data : %f, unit: %ld\r\n", MDR_message.module_id, datapoints[0].entity_id, datapoints[0].data, datapoints[0].unit_id);
+		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+		memset(debug_buf, 0, 128);
+	    }
 	}
 	else if (SOR_buf[0] == 3) {
 	    uint8_t CTS_buf[] = {0x0, 0x1}, len_buf[4], *cmd_buf;
@@ -307,7 +317,7 @@ bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field
 {
     if(ostream!=NULL && field->tag == s2m_MDR_response_subscriptions_tag) {
 	_subscriptions subs;
-	subs.module_id = 0xd;
+	subs.module_id = 0x4;
 	subs.has_module_id=true;
 	subs.has_entity_id=false;
 	subs.has_module_class=false;
@@ -332,6 +342,8 @@ bool encode_datapoint_callback(pb_ostream_t *ostream, const pb_field_t *field, v
 	    _datapoint datapoint = _datapoint_init_zero;
 	    datapoint.entity_id  = 1;
 	    datapoint.data = 20.70+((float)i/100);
+	    datapoint.unit_id = 1;
+	    datapoint.has_unit_id = true;
 	    if (!pb_encode_tag_for_field(ostream, field))
 		return false;
 	    if (!pb_encode_submessage(ostream, _datapoint_fields, &datapoint))
@@ -362,6 +374,36 @@ bool decode_cmd_string_callback(pb_istream_t *istream, const pb_field_t *field,
     return true;
 }
 
+bool decode_data_callback(pb_istream_t *istream, const pb_field_t *field, void **args)
+{
+    _datapoint loc_datapoint = _datapoint_init_zero;
+    _datapoint *datapoint = *args;
+
+    if (!pb_decode(istream, _datapoint_fields, &loc_datapoint))
+	return false;
+
+    datapoint[data_idx].data = datapoint[data_idx].entity_id = 0;
+    
+    datapoint[data_idx].entity_id = loc_datapoint.entity_id;
+    datapoint[data_idx].data      = loc_datapoint.data;
+    
+    if (loc_datapoint.has_channel_id) {
+	datapoint[data_idx].has_channel_id = true;
+	datapoint[data_idx].channel_id     = loc_datapoint.channel_id;
+    }
+    if (loc_datapoint.has_unit_id) {
+	datapoint[data_idx].has_unit_id = true;
+	datapoint[data_idx].unit_id     = loc_datapoint.unit_id;
+    }
+    if (loc_datapoint.has_timestamp) {
+	datapoint[data_idx].has_timestamp = true;
+	datapoint[data_idx].timestamp    = loc_datapoint.timestamp;
+    }
+
+    data_idx++;
+    return true;
+}
+
 /**
  * @brief System Clock Configuration
  * @retval None