Debug block

1 files changed, 34 insertions, 5 deletions

diff --git a/Src/main-hs.c b/Src/main-hs.c
index 19c666f..a36617c 100644
--- a/Src/main-hs.c
+++ b/Src/main-hs.c
@@ -140,7 +140,11 @@ int main(void)
 	    MDR_req_message.record_type = 7; /* Placeholder for default record type */
 	    if(!pb_encode(&MDR_req_stream, m2s_MDR_request_fields, &MDR_req_message)) {
 		// TODO Error handling for encoding fail
-		
+#ifdef DEBUG_ENABLE
+		goto __MDR_REQ_ENC_FAIL;
+	    __MDR_REQ_ENC_FAIL_END:
+		__asm__("nop");
+#endif
 	    }
 	    else {
 		MDR_req_size = MDR_req_stream.bytes_written;
@@ -148,15 +152,20 @@ int main(void)
 		goto __HS_IDLE_TESTING;
 	    __HS_IDLE_TESTING_END:
 		__asm__("nop");
-#endif		
+#endif /* TESTING_ENABLE */
 	    }
 	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)I2C_ADDRESS, (uint8_t*)MDR_req_buf,
 					MDR_req_buf_len, 10000) != HAL_OK) {
 		// Error handling for transmit fail
 		hs_sts = HS_FAILED;
+#ifdef DEBUG_ENABLE
+		goto __HS_MDR_I2C_ERROR;
+	    __HS_MDR_I2C_ERROR_END:
+		__asm__("nop");
+#endif /* DEBUG_ENABLE */
 	    }
 	    else {
-		hs_sts = HS_MDR_ACK; // Placeholder for testing, change this to next step
+		hs_sts = HS_MDR_ACK;
 	    }
 	    free(MDR_req_buf);
 	    break;
@@ -199,10 +208,10 @@ int main(void)
     {
 	goto __TESTING_BLOCK_END;
     __HS_IDLE_TESTING:
-	sprintf((char*)debug_buf, "Encoded length: %d\r\n", MDR_req_size);
+	sprintf((char*)debug_buf, "MDR req length: %d\r\n", MDR_req_size);
 	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
 	memset(debug_buf, 0, 128);
-	uint8_t bufbuf[] = "MDR req buffer: ";	  
+	uint8_t bufbuf[] = "\tMDR req buffer: ";	  
 	HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);
 	for(int x=0; x<MDR_req_size; x++) {
 	    sprintf((char*)debug_buf+x, "%x", MDR_req_buf[x]);
@@ -216,6 +225,26 @@ __TESTING_BLOCK_END:
     __asm__("nop");
 #endif
 
+#ifdef DEBUG_ENABLE
+    {
+	goto __DEBUG_BLOCK_END;
+    __MDR_REQ_ENC_FAIL:
+	sprintf((char*)debug_buf, "MDR reqest encoding error\r\n");
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __MDR_REQ_ENC_FAIL_END;
+    __HS_MDR_I2C_ERROR:
+	sprintf((char*)debug_buf, "Unable to send MDR request. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));
+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);
+	memset(debug_buf, 0, 128);
+	goto __HS_MDR_I2C_ERROR_END;
+	
+    }
+    
+__DEBUG_BLOCK_END:
+    __asm__("nop");
+#endif
+    
     Error_Handler();
     
 #else /* MASTER */