Working MDR merge

1 files changed, 88 insertions, 26 deletions

diff --git a/Src/main.c b/Src/main.c
index 528ed27..87a4a16 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -198,6 +198,7 @@ int main(void)
 	HAL_UART_Transmit(&huart1, term, 2, 100);

 	memset(debug_buf, 0, 128);

 #endif /* TESTING_ENABLE */

+

 	uint32_t MDR_len = 0;

 	pb_istream_t MDR_ACK_istream = pb_istream_from_buffer(MDR_ACK_buf, 2);

 	s2m_MDR_req_ACK MDR_ACK;

@@ -216,32 +217,44 @@ int main(void)
 #endif /* TESTING_ENABLE */

 	}

 

-	uint8_t MDR_CTS_buf[8];

-	m2s_MDR_res_CTS MDR_CTS;	

+	uint8_t MDR_CTS_buf[8] = {0};

+	m2s_MDR_res_CTS MDR_CTS;

 	pb_ostream_t MDR_CTS_ostream = pb_ostream_from_buffer(MDR_CTS_buf, sizeof(MDR_CTS_buf));

 	

-	MDR_CTS.timeout = 1000;

-	if (!pb_encode(MDR_CTS_ostream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {

+	MDR_CTS.timeout = 100;

+	if (!pb_encode(&MDR_CTS_ostream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {

 #ifdef DEBUG_ENABLE

 	    sprintf((char*)debug_buf, "MDR encoding error\r\n");

 	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

 	    memset(debug_buf, 0, 128);

 #endif

 	}

-	else {

-	    if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)I2C_ADDRESS, (uint8_t*)MDR_CTS_buf, m2s_MDR_res_CTS_size, 10000) != HAL_OK) {

+#ifdef TESTING_ENABLE

+	sprintf((char*)debug_buf, "CTS size: %d\r\n", MDR_CTS_ostream.bytes_written);

+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

+	memset(debug_buf, 0, 128);

+	uint8_t ctsbuf[] = "\tCTS buffer: ";

+	for(int x=0; x<2; x++) {

+	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);

+	}

+	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 100);

+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

+	HAL_UART_Transmit(&huart1, term, 2, 100);

+	memset(debug_buf, 0, 128);

+#endif /* TESTING_ENABLE */

+	

+	if (HAL_I2C_Master_Transmit(&hi2c1, (uint16_t)I2C_ADDRESS, (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {

 #ifdef DEBUG_ENABLE

-		sprintf((char*)debug_buf, "Unable to send MDR CTS. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));

-		HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

-		memset(debug_buf, 0, 128);

+	    sprintf((char*)debug_buf, "Unable to send MDR CTS. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));

+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

+	    memset(debug_buf, 0, 128);

 #endif /* DEBUG_ENABLE */

-	    }

-	    HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);

 	}

+	HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);

 

 	uint8_t MDR_buf[256];

 	AF_error_counter = 0;

-	while (HAL_I2C_Master_Receive(&hi2c1, (uint8_t)I2C_ADDRESS, (uint8_t*)MDR_buf, MDR_len, 100) != HAL_OK) {

+	while (HAL_I2C_Master_Receive(&hi2c1, (uint8_t)I2C_ADDRESS, (uint8_t*)MDR_buf, MDR_len, 1000) != HAL_OK) {

 	    if (HAL_I2C_GetError(&hi2c1) != HAL_I2C_ERROR_AF) {

 #ifdef DEBUG_ENABLE

 		sprintf((char*)debug_buf, "Unable to get MDR. I2C error: %ld\r\n", HAL_I2C_GetError(&hi2c1));

@@ -250,24 +263,31 @@ int main(void)
 #endif /* DEBUG_ENABLE */

 		break;

 	    }

-	    if (++AF_error_counter > 1000) {

-		break;

-	    }

+	    AF_error_counter++;

+	    /* if (++AF_error_counter > 1000) { */

+	    /* 	break; */

+	    /* } */

 	}

 

+	#ifdef TESTING_ENABLE

+	sprintf((char*)debug_buf, "MDR trials: %ld\r\n", AF_error_counter);

+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

+	memset(debug_buf, 0, 128);

+	#endif

+	

 #ifdef TESTING_ENABLE

-	uint8_t bufbuf3[] = "Got MDR buffer: ";

-	HAL_UART_Transmit(&huart1, bufbuf3, sizeof(bufbuf2), 100);

+	uint8_t mdrbuf[] = "Got MDR: ";

 	for (int x=0; x<MDR_len; x++) {

-	    sprintf((char*)debug_buf+x, "%x", MDR__buf[x]);

+	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);

 	}

-	HAL_UART_Transmit(&huart1, debug_buf, 2, 100);

+	HAL_UART_Transmit(&huart1, mdrbuf, sizeof(mdrbuf), 100);

+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);  

 	HAL_UART_Transmit(&huart1, term, 2, 100);

 	memset(debug_buf, 0, 128);

-#endif /* TESTING_ENABLE */	

+#endif /* TESTING_ENABLE */

 	

 #else /* MASTER */

-	uint8_t buffer[128], debug_buf[128], term[]="\r\n";

+	uint8_t MDR_buf[128], debug_buf[128], term[]="\r\n";

 	size_t MDR_enc_size;

 	s2m_MDR_response res;

 	res.MDR_version=1.1;

@@ -276,8 +296,9 @@ int main(void)
 	res.entity_id=32;

 

 	res.subscriptions.funcs.encode=encode_subscription_callback;

-	pb_ostream_t ostream = pb_ostream_from_buffer(buffer, sizeof(buffer));

-	if (!pb_encode(&ostream, s2m_MDR_response_fields, &res)){

+	pb_ostream_t ostream = pb_ostream_from_buffer(MDR_buf, sizeof(MDR_buf));

+	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);

@@ -296,7 +317,7 @@ int main(void)
 	uint8_t bufbuf[] = "MDR Buffer: ";	  

 	HAL_UART_Transmit(&huart1, bufbuf, sizeof(bufbuf), 100);

 	for(int x=0; x<MDR_enc_size; x++) {

-	    sprintf((char*)debug_buf+x, "%x", buffer[x]);

+	    sprintf((char*)debug_buf+x, "%x", MDR_buf[x]);

 	}

 	HAL_UART_Transmit(&huart1, debug_buf, MDR_enc_size, 100);  

 	HAL_UART_Transmit(&huart1, term, 2, 100);

@@ -333,7 +354,6 @@ int main(void)
 	    uint8_t debug_buf[] = "Failed to get MDR req\r\n";

 	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

 #endif /* DEBUG_ENABLE */

-	    /* continue; */

 	}

 

 #ifdef TESTING_ENABLE

@@ -353,7 +373,6 @@ int main(void)
 	    uint8_t errbuf[] = "MDR request decoding error\r\n";

 	    HAL_UART_Transmit(&huart1, errbuf, sizeof(errbuf), 100);

 #endif /* DEBUG_ENABLE */

-	    /* continue; */

 	}

 	

 #ifdef TESTING_ENABLE

@@ -370,6 +389,49 @@ int main(void)
 	    memset(debug_buf, 0, 128);

 #endif /* DEBUG_ENABLE */

 	}

+

+	uint8_t MDR_CTS_buf[8];

+	m2s_MDR_res_CTS MDR_CTS;

+	

+	if (HAL_I2C_Slave_Receive(&hi2c1, (uint8_t*)MDR_CTS_buf, 2, 10000) != HAL_OK) {

+#ifdef DEBUG_ENABLE

+	    sprintf((char*)debug_buf, "Failed to get MDR CTS\r\n");

+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

+	    memset(debug_buf, 0, 128);

+#endif /* DEBUG_ENABLE */

+	}

+#ifdef TESTING_ENABLE

+	uint8_t ctsbuf[] = "Got CTS buffer: ";

+	for(int x=0; x<2; x++) {

+	    sprintf((char*)debug_buf+x, "%x", MDR_CTS_buf[x]);

+	}

+	HAL_UART_Transmit(&huart1, ctsbuf, sizeof(ctsbuf), 100);

+	HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

+	HAL_UART_Transmit(&huart1, term, 2, 100);

+	memset(debug_buf, 0, 128);

+#endif /* TESTING_ENABLE */

+	

+	pb_istream_t MDR_CTS_istream = pb_istream_from_buffer(MDR_CTS_buf, 2);

+	if (!pb_decode(&MDR_CTS_istream, m2s_MDR_res_CTS_fields, &MDR_CTS)) {

+#ifdef DEBUG_ENABLE

+	    sprintf((char*)debug_buf, "Failed to decode MDR CTS\r\n");

+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

+	    memset(debug_buf, 0, 128);

+#endif /* DEBUG_ENABLE */

+	    Error_Handler();

+	}

+

+	uint32_t MDR_timeout = MDR_CTS.timeout;

+	if (HAL_I2C_Slave_Transmit(&hi2c1, (uint8_t*)MDR_buf, MDR_enc_size, 10000) !=

+	    HAL_OK) {

+#ifdef DEBUG_ENABLE

+	    sprintf((char*)debug_buf, "Unable to send MDR. I2C error: %ld\r\n",

+		    HAL_I2C_GetError(&hi2c1));

+	    HAL_UART_Transmit(&huart1, debug_buf, sizeof(debug_buf), 100);

+	    memset(debug_buf, 0, 128);

+#endif /* DEBUG_ENABLE */

+	}

+	

 #endif /* MASTER */

 	

 	/* USER CODE END 2 */