1 files changed, 13 insertions, 11 deletions

diff --git a/src/main-slave.c b/src/main-slave.c
index 98b5550..f5979db 100644
--- a/src/main-slave.c
+++ b/src/main-slave.c
@@ -85,7 +85,7 @@ int main(void)
 
 	res.subscriptions.funcs.encode=encode_subscription_callback;
 	pb_ostream_t ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf));
-	if(!pb_encode(&ostream, s2m_MDR_response_fields, &res)){
+	if(!pb_encode(&ostream, s2m_MDR_response_fields, &res)) {
 #ifdef DEBUG_ENABLE
 	    uint8_t err_buf[] = "MDR encoding error\r\n";
 	    HAL_UART_Transmit(&huart1, err_buf, sizeof(err_buf), 100);
@@ -226,13 +226,14 @@ int main(void)
 	}
     }	
 
-    
+    HAL_Delay(1000);
     while (1) {
 	uint8_t SOR_buf[m2s_SOR_size] = {0}, debug_buf[128];
-	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, m2s_SOR_size, 10000) != HAL_OK) {
+	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)SOR_buf, m2s_SOR_size, 100) != HAL_OK) {
 	    sprintf((char*)debug_buf, "Failed to get SOR\r\n");
 	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	    memset(debug_buf, 0, 128);
+	    continue;
 	}
 	else {
 	    sprintf((char*)debug_buf, "Got SOR\r\n");
@@ -245,11 +246,12 @@ int main(void)
 	    sprintf((char*)debug_buf, "SOR decode error\r\n");
 	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	    memset(debug_buf, 0, 128);
+	    continue;
 	}
 	else {
 	    sprintf((char*)debug_buf, "SOR decoded; code: %ld\r\n", SOR_message.SOR_code);
 	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
-	    memset(debug_buf, 0, 128);
+	    memset(debug_buf, 0, 128);	    
 	}
 
 	if (SOR_message.SOR_code == 1) {
@@ -262,7 +264,7 @@ int main(void)
 		sprintf((char*)debug_buf, "Data encoding error\r\n");
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-		Error_Handler();
+		continue;
 	    }
 	    data_enc_size = data_ostream.bytes_written;
 	    
@@ -276,7 +278,7 @@ int main(void)
 		sprintf((char*)debug_buf, "DOC encoding error\r\n");
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-		Error_Handler();
+		continue;
 	    }
 	    
 	    sprintf((char*)debug_buf, "s2m_DOC encoded length: %d\r\n", doc_ostream.bytes_written);
@@ -287,7 +289,7 @@ int main(void)
 		sprintf((char*)debug_buf, "DOC I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-		Error_Handler();
+		continue;
 	    }
 	    else {
 		sprintf((char*)debug_buf, "SENT DOC\r\n");
@@ -301,7 +303,7 @@ int main(void)
 		sprintf((char*)debug_buf, "Failed to get CTS: %ld\r\n", HAL_I2C_GetError(&hi2c1));
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-		Error_Handler();
+		continue;
 	    }
 	    else {
 		sprintf((char*)debug_buf, "Got CTS\r\n");
@@ -313,7 +315,7 @@ int main(void)
 		sprintf((char*)debug_buf, "Data I2C send error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
 		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 		memset(debug_buf, 0, 128);
-		Error_Handler();
+		continue;
 	    }
 	    else {
 		sprintf((char*)debug_buf, "SENT DATA\r\n");
@@ -350,7 +352,7 @@ int main(void)
 	    s2m_MDR_response MDR_message;
 	    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);
 
@@ -364,7 +366,7 @@ int main(void)
 bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg)
 {
     if(ostream!=NULL && field->tag == s2m_MDR_response_subscriptions_tag) {
-	for (int x=0; x<20; x++) {
+	for (int x=0; x<5; x++) {
 	    _subscriptions subs;
 	    subs.module_id = x+1;
 	    subs.has_module_id=true;