Working MDR decode on master

3 files changed, 44 insertions, 4 deletions

diff --git a/Makefile b/Makefile
index c6a1ab6..250032c 100644
--- a/Makefile
+++ b/Makefile
@@ -54,7 +54,7 @@ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c \
 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c \

 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c \

 Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c \

-Src/main-hs.c \

+Src/main.c \

 Src/stm32f4xx_it.c \

 Src/stm32f4xx_hal_msp.c \

 Src/system_stm32f4xx.c \

diff --git a/Src/main-hs.c b/Src/main-hs.c
index da59036..5962b9b 100644
--- a/Src/main-hs.c
+++ b/Src/main-hs.c
@@ -252,7 +252,7 @@ int main(void)
 	case (HS_MDR_MDR):
 	{
 	    MDR_buf = (uint8_t*)malloc(MDR_len);
-	    HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);		
+	    HAL_GPIO_TogglePin(led_GPIO_Port, led_Pin);
 	    AF_error_counter = 0;
 	    while (HAL_I2C_Master_Receive(&hi2c1, (uint8_t)I2C_ADDRESS,
 					  (uint8_t*)MDR_buf, MDR_len, 1000) != HAL_OK) {
diff --git a/Src/main.c b/Src/main.c
index 7130e8f..dc25c5a 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -51,7 +51,7 @@
 #define I2C_ADDRESS 0x30F

 

 /* Macro to toggle between master and slave firmware */

-/* #define MASTER */

+#define MASTER

 /* USER CODE END PM */

 

 /* Private variables ---------------------------------------------------------*/

@@ -60,7 +60,8 @@ I2C_HandleTypeDef hi2c1;
 UART_HandleTypeDef huart1;

 

 /* USER CODE BEGIN PV */

-

+_subscriptions gsubs[10];

+int subs_idx = 0;

 /* USER CODE END PV */

 

 /* Private function prototypes -----------------------------------------------*/

@@ -70,6 +71,7 @@ static void MX_I2C1_Init(void);
 static void MX_USART1_UART_Init(void);

 /* USER CODE BEGIN PFP */

 bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field, void * const *arg);

+bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args);

 /* USER CODE END PFP */

 

 /* Private user code ---------------------------------------------------------*/

@@ -270,6 +272,21 @@ int main(void)
 	HAL_UART_Transmit(&huart1, term, 2, 100);

 	memset(debug_buf, 0, 128);

 #endif /* TESTING_ENABLE */

+

+	s2m_MDR_response MDR_res;

+	MDR_res.subscriptions.funcs.decode=decode_subscriptions_callback;

+	pb_istream_t MDR_istream = pb_istream_from_buffer(MDR_buf, MDR_len);

+	// Decode the mdr

+	if (!pb_decode(&MDR_istream, s2m_MDR_response_fields, &MDR_res)) {

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

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

+	    memset(debug_buf, 0, 128);

+	}

+	else {

+	    sprintf((char*)debug_buf, "MDR:\n\tVersion: %f\nID: %ld\nSub 1 ID: %ld\r\n", MDR_res.MDR_version, MDR_res.module_id, gsubs[1].module_id);

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

+	    memset(debug_buf, 0, 128);

+	}

 	

 #else /* MASTER */

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

@@ -577,6 +594,7 @@ bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field
 	    subs.i2c_address = x+1;

 	    subs.has_entity_id=false;

 	    subs.has_module_class=false;

+	    subs.has_module_id=true;

 	    subs.has_i2c_address=true;

 	    if(!pb_encode_tag_for_field(ostream, field)){

 		printf("ERR1\n");

@@ -593,6 +611,28 @@ bool encode_subscription_callback(pb_ostream_t *ostream, const pb_field_t *field
     }

     return true;

 }

+

+bool decode_subscriptions_callback(pb_istream_t *istream, const pb_field_t *field, void **args)

+{

+    _subscriptions subs;

+    if(!pb_decode(istream, _subscriptions_fields, &subs)) {

+	return false;

+    }

+    else {

+	/* Parse all fields if they're included */

+	if (subs.has_module_id)

+	    gsubs[subs_idx].module_id = subs.module_id;

+	if (subs.has_entity_id)

+	    gsubs[subs_idx].entity_id = subs.entity_id;

+	if (subs.has_module_class)

+	    gsubs[subs_idx].module_class = subs.module_class;

+	if (subs.has_i2c_address)

+	    gsubs[subs_idx].i2c_address = subs.i2c_address;

+	subs_idx++;

+	subs_idx%=10;

+	return true;

+    }

+}

 /* USER CODE END 4 */

 

 /**