From 2a67bbceb4e5fd88fba16b1cd68aa44ca2eebcff Mon Sep 17 00:00:00 2001 From: rdu Date: Wed, 7 Jul 2021 19:03:54 +0800 Subject: [PATCH] saved work --- CMakeLists.txt | 3 + include/ugv_sdk/interface/scout_interface.hpp | 29 ++ include/ugv_sdk/legacy/agilex_base_v1.hpp | 104 +++++++ include/ugv_sdk/legacy/scout_can_parser_v1.h | 42 +++ include/ugv_sdk/legacy/scout_protocol_v1.h | 275 ++++++++++++++++++ src/legacy/scout_can_parser_v1.c | 178 ++++++++++++ 6 files changed, 631 insertions(+) create mode 100644 include/ugv_sdk/interface/scout_interface.hpp create mode 100644 include/ugv_sdk/legacy/agilex_base_v1.hpp create mode 100644 include/ugv_sdk/legacy/scout_can_parser_v1.h create mode 100644 include/ugv_sdk/legacy/scout_protocol_v1.h create mode 100644 src/legacy/scout_can_parser_v1.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 6afef04..f7949ba 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,6 +83,9 @@ add_library(${PROJECT_NAME} src/tracer_base.cpp # src/bunker_base.cpp src/ranger_base.cpp + ######################## + # legacy support + src/legacy/scout_can_parser_v1.c ) target_link_libraries(${PROJECT_NAME} PUBLIC Threads::Threads) target_compile_definitions(${PROJECT_NAME} PUBLIC ASIO_ENABLE_OLD_SERVICES) diff --git a/include/ugv_sdk/interface/scout_interface.hpp b/include/ugv_sdk/interface/scout_interface.hpp new file mode 100644 index 0000000..3ce0d26 --- /dev/null +++ b/include/ugv_sdk/interface/scout_interface.hpp @@ -0,0 +1,29 @@ +/* + * scout_interface.hpp + * + * Created on: Jul 07, 2021 09:10 + * Description: + * + * Copyright (c) 2021 Weston Robot Pte. Ltd. + */ + +#ifndef SCOUT_INTERFACE_HPP +#define SCOUT_INTERFACE_HPP + +namespace westonrobot { +class ScoutInterface { + public: + // set up connection + void Connect(std::string dev_name) override; + + // robot control + void SetMotionCommand(double linear_vel, double angular_vel); + void SetLightCommand(const ScoutLightCmd &cmd); + void DisableLightCmdControl(); + + // get robot state + ScoutState GetScoutState(); +}; +} // namespace westonrobot + +#endif /* SCOUT_INTERFACE_HPP */ diff --git a/include/ugv_sdk/legacy/agilex_base_v1.hpp b/include/ugv_sdk/legacy/agilex_base_v1.hpp new file mode 100644 index 0000000..5fec03e --- /dev/null +++ b/include/ugv_sdk/legacy/agilex_base_v1.hpp @@ -0,0 +1,104 @@ +/* + * agilex_base.hpp + * + * Created on: Dec 22, 2020 17:14 + * Description: + * + * Each robot class derived from this base class should provide implementation + * for the following two functions: + * + * - virtual void Connect(std::string dev_name) = 0; + * - virtual void ParseCANFrame(can_frame *rx_frame) = 0; + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#ifndef AGILEX_BASE_HPP +#define AGILEX_BASE_HPP + +#include +#include +#include +#include +#include + +#include "ugv_sdk/details/async_port/async_can.hpp" +#include "ugv_sdk/agilex_message.h" + +namespace westonrobot { +class AgilexBase { + public: + AgilexBase() = default; + virtual ~AgilexBase(); + + // do not allow copy or assignment + AgilexBase(const AgilexBase &hunter) = delete; + AgilexBase &operator=(const AgilexBase &hunter) = delete; + + // any derived robot must implement this method with proper call back defined + virtual void Connect(std::string dev_name) = 0; + + // cmd thread runs at 50Hz (20ms) by default + void SetCmdThreadPeriodMs(int32_t period_ms); + + // timeout: robot stops if user does not call SetMotionCommand() periodically + void EnableCmdTimeout(uint32_t timeout_ms = 100); + void DisableTimeout() { enable_timeout_ = false; } + + // switch to commanded mode + void EnableCommandedMode(); + + // enforce 50Hz command loop for all AgileX robots internally + void SetMotionCommand(double linear_vel, double angular_vel, + double lateral_velocity, double steering_angle); + + // one-shot light command + void SendLightCommand(LightMode front_mode, uint8_t front_custom_value, + LightMode rear_mode, uint8_t rear_custom_value); + void DisableLightControl(); + + // motion mode change + void SetMotionMode(uint8_t mode); + + // reset fault states + void ResetRobotState(); + + protected: + std::mutex state_mutex_; + std::mutex motion_cmd_mutex_; + MotionCommandMessage current_motion_cmd_; + + // communication interface + bool can_connected_ = false; + std::shared_ptr can_; + + // timeout to be implemented in each vehicle + bool enable_timeout_ = true; + uint32_t timeout_ms_ = 500; + uint32_t watchdog_counter_ = 0; + void FeedCmdTimeoutWatchdog() { watchdog_counter_ = 0; }; + + // command thread + std::thread cmd_thread_; + int32_t cmd_thread_period_ms_ = 20; + bool cmd_thread_started_ = false; + std::atomic keep_running_; + + // internal functions + void StartCmdThread(); + void ControlLoop(int32_t period_ms); + + // connect to roboot from CAN or serial + using CANFrameRxCallback = AsyncCAN::ReceiveCallback; + void Connect(std::string dev_name, CANFrameRxCallback cb); + void Disconnect(); + + // ask background thread to shutdown properly + void Terminate(); + + void SendRobotCmd(); + virtual void ParseCANFrame(can_frame *rx_frame) = 0; +}; +} // namespace westonrobot + +#endif /* AGILEX_BASE_HPP */ diff --git a/include/ugv_sdk/legacy/scout_can_parser_v1.h b/include/ugv_sdk/legacy/scout_can_parser_v1.h new file mode 100644 index 0000000..c465652 --- /dev/null +++ b/include/ugv_sdk/legacy/scout_can_parser_v1.h @@ -0,0 +1,42 @@ +/* + * scout_can_parser.h + * + * Created on: Aug 31, 2019 04:23 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#ifndef SCOUT_CAN_PARSER_H +#define SCOUT_CAN_PARSER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#include "ugv_sdk/legacy/scout_protocol_v1.h" + +#ifdef __linux__ +#include +#else +struct can_frame +{ + uint32_t can_id; + uint8_t can_dlc; + uint8_t data[8]__attribute__((aligned(8))); +}; +#endif + +bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg); +void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame); + +uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc); + +#ifdef __cplusplus +} +#endif + +#endif /* SCOUT_CAN_PARSER_H */ diff --git a/include/ugv_sdk/legacy/scout_protocol_v1.h b/include/ugv_sdk/legacy/scout_protocol_v1.h new file mode 100644 index 0000000..bb0d32b --- /dev/null +++ b/include/ugv_sdk/legacy/scout_protocol_v1.h @@ -0,0 +1,275 @@ +/* + * scout_protocol.h + * + * Created on: Aug 07, 2019 21:49 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#ifndef SCOUT_PROTOCOL_H +#define SCOUT_PROTOCOL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define SCOUT_CMD_BUF_LEN 32 +#define SCOUT_STATUS_BUF_LEN 32 +#define SCOUT_FRAME_SIZE 13 + +#define SCOUT_MOTOR1_ID ((uint8_t)0x00) +#define SCOUT_MOTOR2_ID ((uint8_t)0x01) +#define SCOUT_MOTOR3_ID ((uint8_t)0x02) +#define SCOUT_MOTOR4_ID ((uint8_t)0x03) + +// UART Definitions +#define UART_FRAME_SYSTEM_STATUS_ID ((uint8_t)0x01) +#define UART_FRAME_MOTION_STATUS_ID ((uint8_t)0x02) +#define UART_FRAME_MOTOR1_DRIVER_STATUS_ID ((uint8_t)0x03) +#define UART_FRAME_MOTOR2_DRIVER_STATUS_ID ((uint8_t)0x04) +#define UART_FRAME_MOTOR3_DRIVER_STATUS_ID ((uint8_t)0x05) +#define UART_FRAME_MOTOR4_DRIVER_STATUS_ID ((uint8_t)0x06) +#define UART_FRAME_LIGHT_STATUS_ID ((uint8_t)0x07) + +#define UART_FRAME_MOTION_CONTROL_ID ((uint8_t)0x01) +#define UART_FRAME_LIGHT_CONTROL_ID ((uint8_t)0x02) + +// CAN Definitions +#define CAN_MSG_MOTION_CONTROL_CMD_ID ((uint32_t)0x130) +#define CAN_MSG_MOTION_CONTROL_STATUS_ID ((uint32_t)0x131) +#define CAN_MSG_LIGHT_CONTROL_CMD_ID ((uint32_t)0x140) +#define CAN_MSG_LIGHT_CONTROL_STATUS_ID ((uint32_t)0x141) +#define CAN_MSG_SYSTEM_STATUS_STATUS_ID ((uint32_t)0x151) +#define CAN_MSG_MOTOR1_DRIVER_STATUS_ID ((uint32_t)0x200) +#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x201) +#define CAN_MSG_MOTOR3_DRIVER_STATUS_ID ((uint32_t)0x202) +#define CAN_MSG_MOTOR4_DRIVER_STATUS_ID ((uint32_t)0x203) + +/*--------------------- Control/State Constants ------------------------*/ + +// Motion Control +#define CTRL_MODE_REMOTE ((uint8_t)0x00) +#define CTRL_MODE_CMD_CAN ((uint8_t)0x01) +#define CTRL_MODE_CMD_UART ((uint8_t)0x02) +#define CTRL_MODE_COMMANDED ((uint8_t)0x03) + +#define FAULT_CLR_NONE ((uint8_t)0x00) +#define FAULT_CLR_BAT_UNDER_VOL ((uint8_t)0x01) +#define FAULT_CLR_BAT_OVER_VOL ((uint8_t)0x02) +#define FAULT_CLR_MOTOR1_COMM ((uint8_t)0x03) +#define FAULT_CLR_MOTOR2_COMM ((uint8_t)0x04) +#define FAULT_CLR_MOTOR3_COMM ((uint8_t)0x05) +#define FAULT_CLR_MOTOR4_COMM ((uint8_t)0x06) +#define FAULT_CLR_MOTOR_DRV_OVERHEAT ((uint8_t)0x07) +#define FAULT_CLR_MOTOR_OVERCURRENT ((uint8_t)0x08) + +// Light Control +#define LIGHT_DISABLE_CTRL ((uint8_t)0x00) +#define LIGHT_ENABLE_CTRL ((uint8_t)0x01) + +#define LIGHT_MODE_CONST_OFF ((uint8_t)0x00) +#define LIGHT_MODE_CONST_ON ((uint8_t)0x01) +#define LIGHT_MODE_BREATH ((uint8_t)0x02) +#define LIGHT_MODE_CUSTOM ((uint8_t)0x03) + +// System Status Feedback +#define BASE_STATE_NORMAL ((uint8_t)0x00) +#define BASE_STATE_ESTOP ((uint8_t)0x01) +#define BASE_STATE_EXCEPTION ((uint8_t)0x02) + +#define FAULT_CAN_CHECKSUM_ERROR ((uint16_t)0x0100) +#define FAULT_MOTOR_DRV_OVERHEAT_W ((uint16_t)0x0200) +#define FAULT_MOTOR_OVERCURRENT_W ((uint16_t)0x0400) +#define FAULT_BAT_UNDER_VOL_W ((uint16_t)0x0800) +#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x1000) +#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x2000) +#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x4000) +#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x8000) + +#define FAULT_BAT_UNDER_VOL_F ((uint16_t)0x0001) +#define FAULT_BAT_OVER_VOL_F ((uint16_t)0x0002) +#define FAULT_MOTOR1_COMM_F ((uint16_t)0x0004) +#define FAULT_MOTOR2_COMM_F ((uint16_t)0x0008) +#define FAULT_MOTOR3_COMM_F ((uint16_t)0x0010) +#define FAULT_MOTOR4_COMM_F ((uint16_t)0x0020) +#define FAULT_MOTOR_DRV_OVERHEAT_F ((uint16_t)0x0040) +#define FAULT_MOTOR_OVERCURRENT_F ((uint16_t)0x0080) + +/*-------------------- Control/Feedback Messages -----------------------*/ + +/* No padding in the struct */ +// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect +#pragma pack(push, 1) + +// Note: id could be different for UART and CAN protocol + +// Motion Control +typedef struct { + union + { + struct + { + uint8_t control_mode; + uint8_t fault_clear_flag; + int8_t linear_velocity_cmd; + int8_t angular_velocity_cmd; + uint8_t reserved0; + uint8_t reserved1; + uint8_t count; + uint8_t checksum; + } cmd; + uint8_t raw[8]; + } data; +} MotionControlMessage; + +typedef struct { + union + { + struct + { + struct + { + uint8_t high_byte; + uint8_t low_byte; + } linear_velocity; + struct + { + uint8_t high_byte; + uint8_t low_byte; + } angular_velocity; + uint8_t reserved0; + uint8_t reserved1; + uint8_t count; + uint8_t checksum; + } status; + uint8_t raw[8]; + } data; +} MotionStatusMessage; + +// System Status Feedback +typedef struct { + union + { + struct + { + uint8_t base_state; + uint8_t control_mode; + struct + { + uint8_t high_byte; + uint8_t low_byte; + } battery_voltage; + struct + { + uint8_t high_byte; + uint8_t low_byte; + } fault_code; + uint8_t count; + uint8_t checksum; + } status; + uint8_t raw[8]; + } data; +} SystemStatusMessage; + +// Light Control +typedef struct { + union + { + struct + { + uint8_t light_ctrl_enable; + uint8_t front_light_mode; + uint8_t front_light_custom; + uint8_t rear_light_mode; + uint8_t rear_light_custom; + uint8_t reserved0; + uint8_t count; + uint8_t checksum; + } cmd; + uint8_t raw[8]; + } data; +} LightControlMessage; + +typedef struct { + union + { + struct + { + uint8_t light_ctrl_enable; + uint8_t front_light_mode; + uint8_t front_light_custom; + uint8_t rear_light_mode; + uint8_t rear_light_custom; + uint8_t reserved0; + uint8_t count; + uint8_t checksum; + } status; + uint8_t raw[8]; + } data; +} LightStatusMessage; + +// Motor Driver Feedback +typedef struct +{ + uint8_t motor_id; + union { + struct + { + struct + { + uint8_t high_byte; + uint8_t low_byte; + } current; + struct + { + uint8_t high_byte; + uint8_t low_byte; + } rpm; + int8_t temperature; + uint8_t reserved0; + uint8_t count; + uint8_t checksum; + } status; + uint8_t raw[8]; + } data; +} MotorDriverStatusMessage; + +// For convenience to access status/control message +typedef enum +{ + ScoutMsgNone = 0x00, + // status messages + ScoutMotionStatusMsg = 0x01, + ScoutLightStatusMsg = 0x02, + ScoutSystemStatusMsg = 0x03, + ScoutMotorDriverStatusMsg = 0x04, + // control messages + ScoutMotionControlMsg = 0x21, + ScoutLightControlMsg = 0x22 +} ScoutMsgType; + +typedef struct +{ + ScoutMsgType type; + union { + // status messages + MotionStatusMessage motion_status_msg; + LightStatusMessage light_status_msg; + SystemStatusMessage system_status_msg; + MotorDriverStatusMessage motor_driver_status_msg; + // control messages + MotionControlMessage motion_control_msg; + LightControlMessage light_control_msg; + } body; +} ScoutMessage; + +#pragma pack(pop) + +#ifdef __cplusplus +} +#endif + +#endif /* SCOUT_PROTOCOL_H */ diff --git a/src/legacy/scout_can_parser_v1.c b/src/legacy/scout_can_parser_v1.c new file mode 100644 index 0000000..6db95ca --- /dev/null +++ b/src/legacy/scout_can_parser_v1.c @@ -0,0 +1,178 @@ +/* + * scout_can_parser.c + * + * Created on: Aug 31, 2019 04:25 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#include "ugv_sdk/legacy/scout_can_parser_v1.h" + +#include "string.h" + +static void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame); +static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame); + +bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg) +{ + msg->type = ScoutMsgNone; + + switch (rx_frame->can_id) + { + // in the current implementation, both MsgType and can_frame include 8 * uint8_t + case CAN_MSG_MOTION_CONTROL_STATUS_ID: + { + msg->type = ScoutMotionStatusMsg; + // msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID; + memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_LIGHT_CONTROL_STATUS_ID: + { + msg->type = ScoutLightStatusMsg; + // msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; + memcpy(msg->body.light_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_SYSTEM_STATUS_STATUS_ID: + { + msg->type = ScoutSystemStatusMsg; + // msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID; + memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_MOTOR1_DRIVER_STATUS_ID: + { + msg->type = ScoutMotorDriverStatusMsg; + // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; + msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID; + memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_MOTOR2_DRIVER_STATUS_ID: + { + msg->type = ScoutMotorDriverStatusMsg; + // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; + msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID; + memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_MOTOR3_DRIVER_STATUS_ID: + { + msg->type = ScoutMotorDriverStatusMsg; + // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; + msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID; + memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_MOTOR4_DRIVER_STATUS_ID: + { + msg->type = ScoutMotorDriverStatusMsg; + // msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID; + msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID; + memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + // in the current implementation, both MsgType and can_frame include 8 * uint8_t + case CAN_MSG_MOTION_CONTROL_CMD_ID: + { + msg->type = ScoutMotionControlMsg; + // msg->motion_control_msg.id = CAN_MSG_MOTION_CONTROL_CMD_ID; + memcpy(msg->body.motion_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_LIGHT_CONTROL_CMD_ID: + { + msg->type = ScoutLightControlMsg; + // msg->light_control_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; + memcpy(msg->body.light_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + default: + break; + } + + return true; +} + +void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame) +{ + switch (msg->type) + { + // in the current implementation, both MsgType and can_frame include 8 * uint8_t + case ScoutMotionStatusMsg: + { + tx_frame->can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc); + break; + } + case ScoutLightStatusMsg: + { + tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_STATUS_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.light_status_msg.data.raw, tx_frame->can_dlc); + break; + } + case ScoutSystemStatusMsg: + { + tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc); + break; + } + case ScoutMotorDriverStatusMsg: + { + if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID) + tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; + else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID) + tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID; + else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID) + tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID; + else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID) + tx_frame->can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc); + break; + } + case ScoutMotionControlMsg: + { + EncodeScoutMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame); + break; + } + case ScoutLightControlMsg: + { + EncodeScoutLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame); + break; + } + default: + break; + } + tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); +} + +void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame) +{ + tx_frame->can_id = CAN_MSG_MOTION_CONTROL_CMD_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); + tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); +} + +void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame) +{ + tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_CMD_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); + tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); +} + +uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc) +{ + uint8_t checksum = 0x00; + checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc; + for (int i = 0; i < (dlc - 1); ++i) + checksum += data[i]; + return checksum; +} \ No newline at end of file