From ad58ecee851c98a5d21004379b830eed7b139804 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 29 Nov 2016 09:28:43 +0100 Subject: [PATCH] Heartbeat implementation --- UAV-ControlSystem/src/drivers/arduino_com.c | 37 +++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/UAV-ControlSystem/src/drivers/arduino_com.c b/UAV-ControlSystem/src/drivers/arduino_com.c index 8e0c246..7d97c1b 100644 --- a/UAV-ControlSystem/src/drivers/arduino_com.c +++ b/UAV-ControlSystem/src/drivers/arduino_com.c @@ -161,6 +161,14 @@ arduino_data_t find_packet_from_header(uint8_t header) } +#define BYTE1_32BITS_VALUE(x) ((x & 0xFF000000) >> 24) +#define BYTE2_32BITS_VALUE(x) ((x & 0x00FF0000) >> 16) +#define BYTE3_32BITS_VALUE(x) ((x & 0x0000FF00) >> 8) +#define BYTE4_32BITS_VALUE(x) ((x & 0x000000FF) >> 0) + +const uint32_t heartbeat_msg = 0xDEADBEEF; +const uint32_t heartbeat_rsp = 0xBA1DFACE; + /*********************************************************************** * BRIEF: A function that parses the message byte per byte and then * if it was wrong it will do recursive call and parse the rest @@ -175,6 +183,35 @@ void arduino_parse_message(uint8_t data) static uint8_t secondary_message_it = 0; static arduino_data_t msg_header_and_size = { .size = 0, .dataPtr = NULL }; static uint8_t crc = 0; + static uint8_t heartbeatiterator = 0; + + if (heartbeatiterator == 0 && data == BYTE1_32BITS_VALUE(heartbeat_msg)) + heartbeatiterator = 1; + + if (heartbeatiterator > 0) + { + switch (heartbeatiterator) + { + case 1: + heartbeatiterator = 2; + break; + case 2: + heartbeatiterator = (data == BYTE2_32BITS_VALUE(heartbeat_msg)) ? 3 : 0; + break; + case 3: + heartbeatiterator = (data == BYTE3_32BITS_VALUE(heartbeat_msg)) ? 4 : 0; + break; + case 4: + heartbeatiterator = 0; + if(data == BYTE4_32BITS_VALUE(heartbeat_msg)) + { + usart_transmit(&usartdmaHandler.usart_pro, (uint8_t *) &heartbeat_rsp, 4, 10000); + } + break; + default: + break; + } + } if(find_header) {