From 27ba46a45088c169552252fd63927482fab7d59c Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Thu, 5 Nov 2020 13:35:28 +0800 Subject: [PATCH] removed uart support from protocol v2 --- .../include/ugv_sdk/proto/agx_protocol_v2.h | 12 - ugv_sdk/src/agx_msg_parser.c | 630 ------------------ 2 files changed, 642 deletions(-) diff --git a/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h b/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h index b3b6b94..4b44a47 100644 --- a/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h +++ b/ugv_sdk/include/ugv_sdk/proto/agx_protocol_v2.h @@ -32,18 +32,6 @@ /*--------------------------- Message IDs ------------------------------*/ -// UART Message IDs -#define UART_MSG_SYSTEM_STATUS_ID ((uint8_t)0x01) -#define UART_MSG_MOTION_STATUS_ID ((uint8_t)0x02) -#define UART_MSG_MOTOR1_DRIVER_STATUS_ID ((uint8_t)0x03) -#define UART_MSG_MOTOR2_DRIVER_STATUS_ID ((uint8_t)0x04) -#define UART_MSG_MOTOR3_DRIVER_STATUS_ID ((uint8_t)0x05) -#define UART_MSG_MOTOR4_DRIVER_STATUS_ID ((uint8_t)0x06) -#define UART_MSG_LIGHT_STATUS_ID ((uint8_t)0x07) - -#define UART_MSG_MOTION_CONTROL_ID ((uint8_t)0x01) -#define UART_MSG_LIGHT_CONTROL_ID ((uint8_t)0x02) - // CAN Message IDs #define CAN_MSG_MOTION_COMMAND_ID ((uint32_t)0x111) #define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x121) diff --git a/ugv_sdk/src/agx_msg_parser.c b/ugv_sdk/src/agx_msg_parser.c index 6517830..600c077 100644 --- a/ugv_sdk/src/agx_msg_parser.c +++ b/ugv_sdk/src/agx_msg_parser.c @@ -22,58 +22,6 @@ #include "segger/jlink_rtt.h" #endif -typedef enum { - WAIT_FOR_SOF1 = 0, - WAIT_FOR_SOF2, - WAIT_FOR_FRAME_LEN, - WAIT_FOR_FRAME_TYPE, - WAIT_FOR_FRAME_ID, - WAIT_FOR_PAYLOAD, - WAIT_FOR_FRAME_COUNT, - WAIT_FOR_CHECKSUM -} UartDecodeState; - -#define UART_MSG_BUFFER_SIZE 64 -#define UART_MSG_SOF_LEN ((uint8_t)2) -#define UART_MSG_FIXED_FIELD_LEN ((uint8_t)4) -#define UART_MSG_SOF1 ((uint8_t)0x5a) -#define UART_MSG_SOF2 ((uint8_t)0xa5) -#define UART_MSG_TYPE_CONTROL ((uint8_t)0x55) -#define UART_MSG_TYPE_STATUS ((uint8_t)0xaa) -#define UART_MSG_NONE_ID ((uint8_t)0x00) - -// frame buffer -static struct { - uint8_t frame_id; - uint8_t frame_type; - uint8_t frame_len; - uint8_t frame_cnt; - uint8_t frame_checksum; - uint8_t internal_checksum; - uint8_t payload_buffer[UART_MSG_BUFFER_SIZE]; - size_t payload_data_pos; -} uart_parsing_data; - -// statisctics -typedef struct { - uint32_t frame_parsed; - uint32_t frame_with_wrong_checksum; -} UartParsingStats; - -static UartParsingStats uart_parsing_stats = {.frame_parsed = true, - .frame_with_wrong_checksum = 123}; - -// internal functions -static bool ParseChar(uint8_t c, AgxMessage *msg); -static uint8_t CalcBufferedFrameChecksum(); -static bool ConstructStatusMessage(AgxMessage *msg); -static bool ConstructControlMessage(AgxMessage *msg); - -static void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, - uint8_t *buf, uint8_t *len); -static void EncodeLightControlMsgToUART(const LightControlMessage *msg, - uint8_t *buf, uint8_t *len); - static void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame); static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, @@ -297,581 +245,3 @@ uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc) { for (int i = 0; i < (dlc - 1); ++i) checksum += data[i]; return checksum; } - -void EncodeUartData(const AgxMessage *msg, uint8_t *buf, uint8_t *len) { - // SOF - buf[0] = UART_MSG_SOF1; - buf[1] = UART_MSG_SOF2; - - // frame len, type, ID - buf[2] = 0x0a; - buf[3] = UART_MSG_TYPE_STATUS; - - switch (msg->type) { - // in the current implementation, both MsgType and can_frame include 8 * - // uint8_t - case ScoutMotionStatusMsg: { - buf[4] = UART_MSG_MOTION_STATUS_ID; - buf[5] = msg->body.motion_status_msg.data.cmd.linear_velocity.high_byte; - buf[6] = msg->body.motion_status_msg.data.cmd.linear_velocity.low_byte; - buf[7] = msg->body.motion_status_msg.data.cmd.angular_velocity.high_byte; - buf[8] = msg->body.motion_status_msg.data.cmd.angular_velocity.low_byte; - buf[9] = 0; - buf[10] = 0; - buf[11] = 0; - break; - } - case ScoutLightStatusMsg: { - buf[4] = UART_MSG_LIGHT_STATUS_ID; - buf[5] = msg->body.light_status_msg.data.status.light_ctrl_enable; - buf[6] = msg->body.light_status_msg.data.status.front_light_mode; - buf[7] = msg->body.light_status_msg.data.status.front_light_custom; - buf[8] = msg->body.light_status_msg.data.status.rear_light_mode; - buf[9] = msg->body.light_status_msg.data.status.rear_light_custom; - buf[10] = 0; - buf[11] = msg->body.light_status_msg.data.status.count; - break; - } - case ScoutSystemStatusMsg: { - buf[4] = UART_MSG_SYSTEM_STATUS_ID; - buf[5] = msg->body.system_status_msg.status.vehicle_state; - buf[6] = msg->body.system_status_msg.status.control_mode; - buf[7] = - msg->body.system_status_msg.status.battery_voltage.high_byte; - buf[8] = msg->body.system_status_msg.status.battery_voltage.low_byte; - buf[9] = msg->body.system_status_msg.status.fault_code; - buf[10] = 0; - buf[11] = 0; - break; - } - case ScoutMotorDriverStatusMsg: { - if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID) - buf[4] = UART_MSG_MOTOR1_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID) - buf[4] = UART_MSG_MOTOR2_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID) - buf[4] = UART_MSG_MOTOR3_DRIVER_STATUS_ID; - else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID) - buf[4] = UART_MSG_MOTOR4_DRIVER_STATUS_ID; - buf[5] = msg->body.motor_driver_status_msg.data.status.current.high_byte; - buf[6] = msg->body.motor_driver_status_msg.data.status.current.low_byte; - buf[7] = msg->body.motor_driver_status_msg.data.status.rpm.high_byte; - buf[8] = msg->body.motor_driver_status_msg.data.status.rpm.low_byte; - buf[9] = msg->body.motor_driver_status_msg.data.status.temperature; - buf[10] = 0; - buf[11] = msg->body.motor_driver_status_msg.data.status.count; - break; - } - case ScoutMotionControlMsg: { - EncodeMotionControlMsgToUART(&(msg->body.motion_control_msg), buf, len); - break; - } - case ScoutLightControlMsg: { - EncodeLightControlMsgToUART(&(msg->body.light_control_msg), buf, len); - break; - } - default: - break; - } - - buf[12] = CalcUartDataChecksum(buf, buf[2] + UART_MSG_SOF_LEN); - - // length: SOF + Frame + Checksum - *len = buf[2] + UART_MSG_SOF_LEN + 1; -} - -bool DecodeUartData(uint8_t c, AgxMessage *msg) { - static AgxMessage decoded_msg; - - bool result = ParseChar(c, &decoded_msg); - if (result) *msg = decoded_msg; - return result; -} - -void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, - uint8_t *len) { - // SOF - buf[0] = UART_MSG_SOF1; - buf[1] = UART_MSG_SOF2; - - // frame len, type, ID - buf[2] = 0x0a; - buf[3] = UART_MSG_TYPE_CONTROL; - buf[4] = UART_MSG_MOTION_CONTROL_ID; - - // frame payload - buf[5] = msg->data.cmd.linear_velocity.high_byte; - buf[6] = msg->data.cmd.linear_velocity.low_byte; - buf[7] = msg->data.cmd.angular_velocity.high_byte; - buf[8] = msg->data.cmd.angular_velocity.low_byte; - buf[9] = 0x00; - buf[10] = 0x00; - - // frame count, checksum - buf[11] = 0x00; - buf[12] = 0x00; - - // length: SOF + Frame + Checksum - *len = buf[2] + UART_MSG_SOF_LEN + 1; -} - -void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, - uint8_t *len) { - // SOF - buf[0] = UART_MSG_SOF1; - buf[1] = UART_MSG_SOF2; - - // frame len, type, ID - buf[2] = 0x0a; - buf[3] = UART_MSG_TYPE_CONTROL; - buf[4] = UART_MSG_LIGHT_CONTROL_ID; - - // frame payload - buf[5] = msg->data.cmd.light_ctrl_enable; - buf[6] = msg->data.cmd.front_light_mode; - buf[7] = msg->data.cmd.front_light_custom; - buf[8] = msg->data.cmd.rear_light_mode; - buf[9] = msg->data.cmd.rear_light_custom; - buf[10] = 0x00; - - // frame count, checksum - buf[11] = msg->data.cmd.count; - buf[12] = CalcUartDataChecksum(buf, buf[2] + UART_MSG_SOF_LEN); - - // length: SOF + Frame + Checksum - *len = buf[2] + UART_MSG_SOF_LEN + 1; -} - -bool ParseChar(uint8_t c, AgxMessage *msg) { - static UartDecodeState decode_state = WAIT_FOR_SOF1; - - bool new_frame_parsed = false; - switch (decode_state) { - case WAIT_FOR_SOF1: { - if (c == UART_MSG_SOF1) { - uart_parsing_data.frame_id = UART_MSG_NONE_ID; - uart_parsing_data.frame_type = 0; - uart_parsing_data.frame_len = 0; - uart_parsing_data.frame_cnt = 0; - uart_parsing_data.frame_checksum = 0; - uart_parsing_data.internal_checksum = 0; - uart_parsing_data.payload_data_pos = 0; - memset(uart_parsing_data.payload_buffer, 0, UART_MSG_BUFFER_SIZE); - - decode_state = WAIT_FOR_SOF2; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "found sof1" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "found sof1\n"); -#endif - } - break; - } - case WAIT_FOR_SOF2: { - if (c == UART_MSG_SOF2) { - decode_state = WAIT_FOR_FRAME_LEN; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "found sof2" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "found sof2\n"); -#endif - } else { - decode_state = WAIT_FOR_SOF1; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "failed to find sof2" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "failed to find sof2\n"); -#endif - } - break; - } - case WAIT_FOR_FRAME_LEN: { - uart_parsing_data.frame_len = c; - decode_state = WAIT_FOR_FRAME_TYPE; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "frame len: " << std::hex << static_cast(frame_len) - << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "frame len: %d\n", frame_len); -#endif - break; - } - case WAIT_FOR_FRAME_TYPE: { - switch (c) { - case UART_MSG_TYPE_CONTROL: { - uart_parsing_data.frame_type = UART_MSG_TYPE_CONTROL; - decode_state = WAIT_FOR_FRAME_ID; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "control type frame received" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "control type frame received\n"); -#endif - break; - } - case UART_MSG_TYPE_STATUS: { - uart_parsing_data.frame_type = UART_MSG_TYPE_STATUS; - decode_state = WAIT_FOR_FRAME_ID; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "status type frame received" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "status type frame received\n"); -#endif - break; - } - default: { -#ifdef PRINT_CPP_DEBUG_INFO - std::cerr << "ERROR: Not expecting frame of a type other than " - "UART_MSG_TYPE_STATUS" - << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, - "ERROR: Not expecting frame of a type other than " - "UART_MSG_TYPE_STATUS\n"); -#endif - decode_state = WAIT_FOR_SOF1; - } - } - break; - } - case WAIT_FOR_FRAME_ID: { - switch (c) { - case UART_MSG_SYSTEM_STATUS_ID: - case UART_MSG_MOTION_STATUS_ID: - case UART_MSG_MOTOR1_DRIVER_STATUS_ID: - case UART_MSG_MOTOR2_DRIVER_STATUS_ID: - case UART_MSG_MOTOR3_DRIVER_STATUS_ID: - case UART_MSG_MOTOR4_DRIVER_STATUS_ID: - case UART_MSG_LIGHT_STATUS_ID: { - uart_parsing_data.frame_id = c; - decode_state = WAIT_FOR_PAYLOAD; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "frame id: " << std::hex << static_cast(frame_id) - << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "frame id: %d\n", frame_id); -#endif - break; - } - default: { -#ifdef PRINT_CPP_DEBUG_INFO - std::cerr << "ERROR: Unknown frame id" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "ERROR: Unknown frame id\n"); -#endif - decode_state = WAIT_FOR_SOF1; - } - } - break; - } - case WAIT_FOR_PAYLOAD: { - uart_parsing_data.payload_buffer[uart_parsing_data.payload_data_pos++] = - c; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "1 byte added: " << std::hex << static_cast(c) - << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "1 byte added: %d\n", c); -#endif - if (uart_parsing_data.payload_data_pos == - (uart_parsing_data.frame_len - UART_MSG_FIXED_FIELD_LEN)) - decode_state = WAIT_FOR_FRAME_COUNT; - break; - } - case WAIT_FOR_FRAME_COUNT: { - uart_parsing_data.frame_cnt = c; - decode_state = WAIT_FOR_CHECKSUM; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "frame count: " << std::hex << static_cast(frame_cnt) - << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "frame count: %d\n", frame_cnt); -#endif - break; - } - case WAIT_FOR_CHECKSUM: { - uart_parsing_data.frame_checksum = c; - uart_parsing_data.internal_checksum = CalcBufferedFrameChecksum(); - new_frame_parsed = true; - decode_state = WAIT_FOR_SOF1; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "--- frame checksum: " << std::hex - << static_cast(frame_checksum) << std::dec << std::endl; - std::cout << "--- internal frame checksum: " << std::hex - << static_cast(internal_checksum) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkRTTPrintf(0, "--- frame checksum: : %d\n", frame_checksum); - JLinkRTTPrintf(0, "--- internal frame checksum: : %d\n", - internal_checksum); -#endif - break; - } - default: - break; - } - - if (new_frame_parsed) { - if (uart_parsing_data.frame_checksum == - uart_parsing_data.internal_checksum) { -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "checksum correct" << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "checksum correct\n"); -#endif - if (uart_parsing_data.frame_type == UART_MSG_TYPE_STATUS) - ConstructStatusMessage(msg); - else if (uart_parsing_data.frame_type == UART_MSG_TYPE_CONTROL) - ConstructControlMessage(msg); - ++uart_parsing_stats.frame_parsed; - } else { - ++uart_parsing_stats.frame_with_wrong_checksum; -#ifdef PRINT_CPP_DEBUG_INFO - std::cout << "checksum is NOT correct" << std::endl; - std::cout << std::hex << static_cast(frame_id) << " , " - << static_cast(frame_len) << " , " - << static_cast(frame_cnt) << " , " - << static_cast(frame_checksum) << " : " << std::dec - << std::endl; - std::cout << "payload: "; - for (int i = 0; i < payload_data_pos; ++i) - std::cout << std::hex << static_cast(payload_buffer[i]) << std::dec - << " "; - std::cout << std::endl; - std::cout << "--- frame checksum: " << std::hex - << static_cast(frame_checksum) << std::dec << std::endl; - std::cout << "--- internal frame checksum: " << std::hex - << static_cast(internal_checksum) << std::dec << std::endl; -#elif (defined(PRINT_JLINK_DEBUG_INFO)) - JLinkWriteString(0, "checksum is NOT correct\n"); -#endif - } - } - - return new_frame_parsed; -} - -bool ConstructControlMessage(AgxMessage *msg) { - if (msg == NULL) return false; - - switch (uart_parsing_data.frame_id) { - case UART_MSG_MOTION_CONTROL_ID: { - msg->type = ScoutMotionControlMsg; - msg->body.motion_control_msg.data.cmd.linear_velocity.high_byte = - uart_parsing_data.payload_buffer[0]; - msg->body.motion_control_msg.data.cmd.linear_velocity.low_byte = - uart_parsing_data.payload_buffer[1]; - msg->body.motion_control_msg.data.cmd.angular_velocity.high_byte = - uart_parsing_data.payload_buffer[2]; - msg->body.motion_control_msg.data.cmd.angular_velocity.low_byte = - uart_parsing_data.payload_buffer[3]; - msg->body.motion_control_msg.data.cmd.reserved0 = - uart_parsing_data.payload_buffer[4]; - msg->body.motion_control_msg.data.cmd.reserved1 = - uart_parsing_data.payload_buffer[5]; - msg->body.motion_control_msg.data.cmd.reserved2 = - uart_parsing_data.payload_buffer[6]; - msg->body.motion_control_msg.data.cmd.reserved3 = - uart_parsing_data.payload_buffer[7]; - break; - } - case UART_MSG_LIGHT_CONTROL_ID: { - msg->type = ScoutLightControlMsg; - msg->body.light_control_msg.data.cmd.light_ctrl_enable = - uart_parsing_data.payload_buffer[0]; - msg->body.light_control_msg.data.cmd.front_light_mode = - uart_parsing_data.payload_buffer[1]; - msg->body.light_control_msg.data.cmd.front_light_custom = - uart_parsing_data.payload_buffer[2]; - msg->body.light_control_msg.data.cmd.rear_light_mode = - uart_parsing_data.payload_buffer[3]; - msg->body.light_control_msg.data.cmd.rear_light_custom = - uart_parsing_data.payload_buffer[4]; - msg->body.light_control_msg.data.cmd.reserved0 = - uart_parsing_data.payload_buffer[5]; - msg->body.light_control_msg.data.cmd.count = uart_parsing_data.frame_cnt; - // msg->body.light_control_msg.data.cmd.checksum = - // uart_parsing_data.frame_checksum; - break; - } - } - return true; -} - -bool ConstructStatusMessage(AgxMessage *msg) { - if (msg == NULL) return false; - - switch (uart_parsing_data.frame_id) { - case UART_MSG_SYSTEM_STATUS_ID: { - msg->type = ScoutSystemStatusMsg; - msg->body.system_status_msg.status.vehicle_state = - uart_parsing_data.payload_buffer[0]; - msg->body.system_status_msg.status.control_mode = - uart_parsing_data.payload_buffer[1]; - msg->body.system_status_msg.status.battery_voltage.high_byte = - uart_parsing_data.payload_buffer[2]; - msg->body.system_status_msg.status.battery_voltage.low_byte = - uart_parsing_data.payload_buffer[3]; - msg->body.system_status_msg.status.fault_code = - uart_parsing_data.payload_buffer[4]; - msg->body.system_status_msg.status.reserved0 = - uart_parsing_data.payload_buffer[5]; - msg->body.system_status_msg.status.reserved1 = - uart_parsing_data.frame_cnt; - msg->body.system_status_msg.status.count = - uart_parsing_data.frame_checksum; - break; - } - case UART_MSG_MOTION_STATUS_ID: { - msg->type = ScoutMotionStatusMsg; - msg->body.motion_status_msg.data.cmd.linear_velocity.high_byte = - uart_parsing_data.payload_buffer[0]; - msg->body.motion_status_msg.data.cmd.linear_velocity.low_byte = - uart_parsing_data.payload_buffer[1]; - msg->body.motion_status_msg.data.cmd.angular_velocity.high_byte = - uart_parsing_data.payload_buffer[2]; - msg->body.motion_status_msg.data.cmd.angular_velocity.low_byte = - uart_parsing_data.payload_buffer[3]; - msg->body.motion_status_msg.data.cmd.reserved0 = 0x00; - msg->body.motion_status_msg.data.cmd.reserved1 = 0x00; - msg->body.motion_status_msg.data.cmd.reserved2 = 0x00; - msg->body.motion_status_msg.data.cmd.reserved3 = 0x00; - break; - } - case UART_MSG_MOTOR1_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID; - msg->body.motor_driver_status_msg.data.status.current.high_byte = - uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = - uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = - uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = - uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = - uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = - uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = - uart_parsing_data.frame_checksum; - break; - } - case UART_MSG_MOTOR2_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID; - msg->body.motor_driver_status_msg.data.status.current.high_byte = - uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = - uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = - uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = - uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = - uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = - uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = - uart_parsing_data.frame_checksum; - break; - } - case UART_MSG_MOTOR3_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID; - msg->body.motor_driver_status_msg.data.status.current.high_byte = - uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = - uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = - uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = - uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = - uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = - uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = - uart_parsing_data.frame_checksum; - break; - } - case UART_MSG_MOTOR4_DRIVER_STATUS_ID: { - msg->type = ScoutMotorDriverStatusMsg; - msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID; - msg->body.motor_driver_status_msg.data.status.current.high_byte = - uart_parsing_data.payload_buffer[0]; - msg->body.motor_driver_status_msg.data.status.current.low_byte = - uart_parsing_data.payload_buffer[1]; - msg->body.motor_driver_status_msg.data.status.rpm.high_byte = - uart_parsing_data.payload_buffer[2]; - msg->body.motor_driver_status_msg.data.status.rpm.low_byte = - uart_parsing_data.payload_buffer[3]; - msg->body.motor_driver_status_msg.data.status.temperature = - uart_parsing_data.payload_buffer[4]; - msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00; - msg->body.motor_driver_status_msg.data.status.count = - uart_parsing_data.frame_cnt; - msg->body.motor_driver_status_msg.data.status.checksum = - uart_parsing_data.frame_checksum; - break; - } - case UART_MSG_LIGHT_STATUS_ID: { - msg->type = ScoutLightStatusMsg; - msg->body.light_status_msg.data.status.light_ctrl_enable = - uart_parsing_data.payload_buffer[0]; - msg->body.light_status_msg.data.status.front_light_mode = - uart_parsing_data.payload_buffer[1]; - msg->body.light_status_msg.data.status.front_light_custom = - uart_parsing_data.payload_buffer[2]; - msg->body.light_status_msg.data.status.rear_light_mode = - uart_parsing_data.payload_buffer[3]; - msg->body.light_status_msg.data.status.rear_light_custom = - uart_parsing_data.payload_buffer[4]; - msg->body.light_status_msg.data.status.reserved0 = 0x00; - msg->body.light_status_msg.data.status.count = - uart_parsing_data.frame_cnt; - msg->body.light_status_msg.data.status.reserved1 = 0x00; - break; - } - } - return true; -} - -uint8_t CalcUartDataChecksum(uint8_t *buf, uint8_t len) { - uint8_t checksum = 0; - -#ifdef USE_XOR_CHECKSUM - for (int i = 0; i < len; ++i) checksum ^= buf[i]; -#else - for (int i = 0; i < len; ++i) checksum += buf[i]; -#endif - - return checksum; -} - -uint8_t CalcBufferedFrameChecksum() { - uint8_t checksum = 0x00; - -#ifdef USE_XOR_CHECKSUM - checksum ^= UART_MSG_SOF1; - checksum ^= UART_MSG_SOF2; - checksum ^= uart_parsing_data.frame_len; - checksum ^= uart_parsing_data.frame_type; - checksum ^= uart_parsing_data.frame_id; - for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i) - checksum ^= uart_parsing_data.payload_buffer[i]; - checksum ^= uart_parsing_data.frame_cnt; -#else - checksum += UART_MSG_SOF1; - checksum += UART_MSG_SOF2; - checksum += uart_parsing_data.frame_len; - checksum += uart_parsing_data.frame_type; - checksum += uart_parsing_data.frame_id; - for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i) - checksum += uart_parsing_data.payload_buffer[i]; - checksum += uart_parsing_data.frame_cnt; -#endif - - return checksum; -}