diff --git a/docs/HUNTER_UserManual_v1.2.6_S.pdf b/docs/HUNTER_UserManual_v1.2.6_S.pdf new file mode 100644 index 0000000..4fa0556 Binary files /dev/null and b/docs/HUNTER_UserManual_v1.2.6_S.pdf differ diff --git a/docs/SCOUT_UserManual_v1.2.10_S.pdf b/docs/SCOUT_UserManual_v1.2.16_S.pdf similarity index 65% rename from docs/SCOUT_UserManual_v1.2.10_S.pdf rename to docs/SCOUT_UserManual_v1.2.16_S.pdf index 95df6c2..d07af84 100644 Binary files a/docs/SCOUT_UserManual_v1.2.10_S.pdf and b/docs/SCOUT_UserManual_v1.2.16_S.pdf differ diff --git a/docs/TRACER_UserManual_v1.1.pdf b/docs/TRACER_UserManual_v1.1.pdf new file mode 100644 index 0000000..4b2ddd7 Binary files /dev/null and b/docs/TRACER_UserManual_v1.1.pdf differ diff --git a/docs/misc/SCOUT_UserManual_v1.2.8_CN.pdf b/docs/misc/SCOUT_UserManual_v1.2.8_CN.pdf deleted file mode 100644 index 4f34c71..0000000 Binary files a/docs/misc/SCOUT_UserManual_v1.2.8_CN.pdf and /dev/null differ diff --git a/docs/misc/check_endianness.md b/docs/misc/check_endianness.md deleted file mode 100644 index 7f8545b..0000000 --- a/docs/misc/check_endianness.md +++ /dev/null @@ -1,36 +0,0 @@ -On a Big Endian-System (Solaris on SPARC) - -``` -$ echo -n I | od -to2 | head -n1 | cut -f2 -d" " | cut -c6 -0 -``` -On a little endian system (Linux on x86) -``` -$ echo -n I | od -to2 | head -n1 | cut -f2 -d" " | cut -c6 -1 -``` -The solution above is clever and works great for Linux *86 and Solaris Sparc. - -I needed a shell-only (no Perl) solution that also worked on AIX/Power and HPUX/Itanium. Unfortunately the last two don't play nice: AIX reports "6" and HPUX gives an empty line. - -Using your solution, I was able to craft something that worked on all these Unix systems: -``` -$ echo I | tr -d [:space:] | od -to2 | head -n1 | awk '{print $2}' | cut -c6 -``` -Regarding the Python solution someone posted, it does not work in Jython because the JVM treats everything as Big. If anyone can get it to work in Jython, please post! - -Also, I found this, which explains the endianness of various platforms. Some hardware can operate in either mode depending on what the O/S selects: http://labs.hoffmanlabs.com/node/544 - -If you're going to use awk this line can be simplified to: -``` -echo -n I | od -to2 | awk '{ print substr($2,6,1); exit}' -``` -For small Linux boxes that don't have 'od' (say OpenWrt) then try 'hexdump': -``` -echo -n I | hexdump -o | awk '{ print substr($2,6,1); exit}' -``` - -Reference: - -* [1] https://serverfault.com/questions/163487/how-to-tell-if-a-linux-system-is-big-endian-or-little-endian -* [2] https://wiki.rdu.im/_pages/Knowledge-Base/Computing/Computing.html \ No newline at end of file diff --git a/docs/misc/scout_interface.png b/docs/misc/scout_interface.png deleted file mode 100755 index 4c23fc4..0000000 Binary files a/docs/misc/scout_interface.png and /dev/null differ diff --git a/docs/misc/scout_monitor_tool.gif b/docs/misc/scout_monitor_tool.gif deleted file mode 100644 index 886aab3..0000000 Binary files a/docs/misc/scout_monitor_tool.gif and /dev/null differ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 72d4c8d..e98dce6 100755 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,5 +1,6 @@ # Add source directories add_subdirectory(apps) add_subdirectory(common) -add_subdirectory(hunter_sdk) add_subdirectory(scout_sdk) +add_subdirectory(hunter_sdk) +add_subdirectory(tracer_sdk) diff --git a/src/apps/CMakeLists.txt b/src/apps/CMakeLists.txt index 45b4def..5c79ed0 100755 --- a/src/apps/CMakeLists.txt +++ b/src/apps/CMakeLists.txt @@ -11,6 +11,9 @@ target_link_libraries(app_scout_demo scoutbase) add_executable(app_hunter_demo hunter_demo/hunter_demo.cpp) target_link_libraries(app_hunter_demo hunterbase) +add_executable(app_tracer_demo tracer_demo/tracer_demo.cpp) +target_link_libraries(app_tracer_demo tracerbase) + if(BUILD_MONITOR) add_subdirectory(scout_monitor) endif() \ No newline at end of file diff --git a/src/apps/tracer_demo/tracer_demo.cpp b/src/apps/tracer_demo/tracer_demo.cpp new file mode 100644 index 0000000..30e3548 --- /dev/null +++ b/src/apps/tracer_demo/tracer_demo.cpp @@ -0,0 +1,108 @@ +/* + * demo_tracer_can.cpp + * + * Created on: Jun 12, 2019 05:03 + * Description: + * + * Copyright (c) 2019 Ruixiang Du (rdu) + */ + +#include "tracer_base/tracer_base.hpp" + +using namespace wescore; + +int main(int argc, char **argv) +{ + std::string device_name; + int32_t baud_rate = 0; + + if (argc == 2) + { + device_name = {argv[1]}; + std::cout << "Specified CAN: " << device_name << std::endl; + } + else + { + std::cout << "Usage: app_tracer_demo " << std::endl + << "Example 1: ./app_tracer_demo can0" << std::endl; + return -1; + } + + TracerBase tracer; + tracer.Connect(device_name); + + // light control + std::cout << "Light: const off" << std::endl; + tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_OFF, 0, TracerLightCmd::LightMode::CONST_OFF, 0}); + sleep(3); + std::cout << "Light: const on" << std::endl; + tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_ON, 0, TracerLightCmd::LightMode::CONST_ON, 0}); + sleep(3); + std::cout << "Light: breath" << std::endl; + tracer.SetLightCommand({TracerLightCmd::LightMode::BREATH, 0, TracerLightCmd::LightMode::BREATH, 0}); + sleep(3); + std::cout << "Light: custom 90-80" << std::endl; + tracer.SetLightCommand({TracerLightCmd::LightMode::CUSTOM, 90, TracerLightCmd::LightMode::CUSTOM, 80}); + sleep(3); + std::cout << "Light: diabled cmd control" << std::endl; + tracer.DisableLightCmdControl(); + + int count = 0; + while (true) + { + // motion control + if (count < 5) + { + std::cout << "Motor: 0.2, 0.0" << std::endl; + tracer.SetMotionCommand(0.2, 0.0); + } + else if (count < 10) + { + std::cout << "Motor: 0.8, 0.3" << std::endl; + tracer.SetMotionCommand(0.8, 0.3); + } + else if (count < 15) + { + std::cout << "Motor: 1.5, 0.5" << std::endl; + tracer.SetMotionCommand(1.5, 0.5); + } + else if (count < 20) + { + std::cout << "Motor: 1.0, 0.3" << std::endl; + tracer.SetMotionCommand(1.0, 0.3); + } + else if (count < 25) + { + std::cout << "Motor: 0.0, 0.0" << std::endl; + tracer.SetMotionCommand(0.0, 0.0); + } + else if (count < 30) + { + std::cout << "Motor: -0.5, -0.3" << std::endl; + tracer.SetMotionCommand(-0.5, -0.3); + } + else if (count < 35) + { + std::cout << "Motor: -1.0, -0.5" << std::endl; + tracer.SetMotionCommand(-1.0, -0.5); + } + else if (count < 40) + { + std::cout << "Motor: 0.0, 0.0," << std::endl; + tracer.SetMotionCommand(0.0, 0.0); + } + + auto state = tracer.GetTracerState(); + std::cout << "-------------------------------" << std::endl; + std::cout << "count: " << count << std::endl; + std::cout << "control mode: " << static_cast(state.control_mode) << " , base state: " << static_cast(state.base_state) << std::endl; + std::cout << "battery voltage: " << state.battery_voltage << std::endl; + std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.angular_velocity << std::endl; + std::cout << "-------------------------------" << std::endl; + + sleep(1); + ++count; + } + + return 0; +} \ No newline at end of file diff --git a/src/hunter_sdk/hunter_base/CMakeLists.txt b/src/hunter_sdk/hunter_base/CMakeLists.txt index b4786a9..63d58ea 100755 --- a/src/hunter_sdk/hunter_base/CMakeLists.txt +++ b/src/hunter_sdk/hunter_base/CMakeLists.txt @@ -9,7 +9,7 @@ target_include_directories(hunterbase PUBLIC $ PRIVATE src) - ## Add executables -# if(BUILD_TESTS) -# add_subdirectory(tests) -# endif() \ No newline at end of file +## Add executables +if(BUILD_TESTS) + add_subdirectory(tests) +endif() \ No newline at end of file diff --git a/src/hunter_sdk/hunter_base/tests/CMakeLists.txt b/src/hunter_sdk/hunter_base/tests/CMakeLists.txt index 2dd735c..df51975 100644 --- a/src/hunter_sdk/hunter_base/tests/CMakeLists.txt +++ b/src/hunter_sdk/hunter_base/tests/CMakeLists.txt @@ -2,15 +2,5 @@ #find_package(LIBRARY_NAME REQUIRED) # tests -add_executable(test_scout_base test_scout_base.cpp) -target_link_libraries(test_scout_base scoutbase) - -# add_executable(test_scout_serial test_scout_serial.cpp) -# target_link_libraries(test_scout_serial scoutbase) - -# add_executable(test_can_msg test_can_msg.cpp) -# target_link_libraries(test_can_msg scoutbase) - -# add_executable(test_serial_parser test_serial_parser.cpp) -# target_link_libraries(test_serial_parser scoutbase) - +add_executable(test_hunter_base test_hunter_base.cpp) +target_link_libraries(test_hunter_base hunterbase) diff --git a/src/hunter_sdk/hunter_base/tests/test_can_msg.cpp b/src/hunter_sdk/hunter_base/tests/test_can_msg.cpp deleted file mode 100644 index a86648b..0000000 --- a/src/hunter_sdk/hunter_base/tests/test_can_msg.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include - -#include "scout_base/details/scout_can_parser.hpp" - -using namespace wescore; - -void print_msg(uint8_t data[8]) -{ - for (int i = 0; i < 8; ++i) - std::cout << std::hex << static_cast(data[i]) << " "; - std::cout << std::dec << std::endl; -} - -int main() -{ - MotionControlMessage msg; - msg.msg.cmd.control_mode = CTRL_MODE_CMD_CAN; - msg.msg.cmd.fault_clear_flag = FAULT_CLR_NONE; - msg.msg.cmd.linear_velocity_cmd = 10; - msg.msg.cmd.angular_velocity_cmd = 0; - msg.msg.cmd.reserved0 = 0; - msg.msg.cmd.reserved1 = 0; - msg.msg.cmd.count = 0; - msg.msg.cmd.checksum = ScoutCANParser::Agilex_CANMsgChecksum(ScoutCANParser::CAN_MSG_MOTION_CONTROL_CMD_ID, msg.msg.raw, msg.len); - print_msg(msg.msg.raw); - - LightControlMessage msg2; - msg2.msg.cmd.light_ctrl_enable = LIGHT_DISABLE_CTRL; - msg2.msg.cmd.front_light_mode = LIGHT_MODE_CONST_ON; - msg2.msg.cmd.front_light_custom = 0; - msg2.msg.cmd.rear_light_mode = LIGHT_MODE_CONST_ON; - msg2.msg.cmd.rear_light_custom = 0; - msg2.msg.cmd.reserved0 = 0; - msg2.msg.cmd.count = 0; - msg2.msg.cmd.checksum = ScoutCANParser::Agilex_CANMsgChecksum(ScoutCANParser::CAN_MSG_LIGHT_CONTROL_CMD_ID, msg2.msg.raw, msg2.len); - print_msg(msg2.msg.raw); - - return 0; -} \ No newline at end of file diff --git a/src/hunter_sdk/hunter_base/tests/test_hunter_base.cpp b/src/hunter_sdk/hunter_base/tests/test_hunter_base.cpp new file mode 100644 index 0000000..3762e9d --- /dev/null +++ b/src/hunter_sdk/hunter_base/tests/test_hunter_base.cpp @@ -0,0 +1,54 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "scout_base/scout_base.hpp" + +#define TEST_WITHOUT_SERIAL_HARDWARE + +using namespace wescore; + +int main(int argc, char **argv) { + std::string device_name; + int32_t baud_rate = 0; + + if (argc == 2) { + device_name = {argv[1]}; + std::cout << "Specified CAN: " << device_name << std::endl; + } else { + std::cout << "Usage: app_scout_demo " << std::endl + << "Example 1: ./app_scout_demo can0" << std::endl; + return -1; + } + + HunterBase scout; + scout.Connect(device_name); + + int count = 0; + while (true) { + std::cout << "Motor: 0.0, 0.0," << std::endl; + scout.SetMotionCommand(0.0, 0.0); + + auto state = scout.GetHunterState(); + std::cout << "-------------------------------" << std::endl; + std::cout << "count: " << count << std::endl; + std::cout << "control mode: " << static_cast(state.control_mode) + << " , base state: " << static_cast(state.base_state) + << std::endl; + std::cout << "battery voltage: " << state.battery_voltage << std::endl; + std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " + << state.angular_velocity << std::endl; + std::cout << "-------------------------------" << std::endl; + + sleep(1); + ++count; + } + + return 0; +} \ No newline at end of file diff --git a/src/hunter_sdk/hunter_base/tests/test_scout_serial.cpp b/src/hunter_sdk/hunter_base/tests/test_scout_serial.cpp deleted file mode 100644 index bb1441f..0000000 --- a/src/hunter_sdk/hunter_base/tests/test_scout_serial.cpp +++ /dev/null @@ -1,45 +0,0 @@ -#include - -#include "scout_base/details/scout_serial_parser.hpp" - -using namespace wescore; - -#include "scout_base/scout_base.hpp" - -#define TEST_WITHOUT_SERIAL_HARDWARE - -using namespace wescore; - -int main(int argc, char **argv) -{ - ScoutBase scout; - // scout.ConfigureCANBus("can1"); - scout.Connect("/dev/ttyUSB0", 115200); - // scout.StartCmdThread(10); - - // scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0}); - - int count = 0; - while (true) - { - // scout.SetMotionCommand(0.5, 0.2); - - // if(count == 10) - // { - // // scout.SetLightCommand({ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0, ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0}); - // scout.DisableLightCmdControl(); - // } - - auto state = scout.GetScoutState(); - std::cout << "-------------------------------" << std::endl; - std::cout << "control mode: " << static_cast(state.control_mode) << " , base state: " << static_cast(state.base_state) << std::endl; - std::cout << "battery voltage: " << state.battery_voltage << std::endl; - std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.angular_velocity << std::endl; - std::cout << "-------------------------------" << std::endl; - - sleep(1); - ++count; - } - - return 0; -} \ No newline at end of file diff --git a/src/hunter_sdk/hunter_base/tests/test_serial_parser.cpp b/src/hunter_sdk/hunter_base/tests/test_serial_parser.cpp deleted file mode 100644 index 0a97de1..0000000 --- a/src/hunter_sdk/hunter_base/tests/test_serial_parser.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include - -#include "scout_base/details/scout_serial_parser.hpp" - -using namespace wescore; - -void print_msg(uint8_t data[8]) -{ - for (int i = 0; i < 8; ++i) - std::cout << std::hex << static_cast(data[i]) << " "; - std::cout << std::dec << std::endl; -} - -uint8_t calc_checksum(uint8_t *buf, uint8_t len) -{ - uint8_t checksum = 0; - for (int i = 0; i < len; ++i) - checksum ^= buf[i]; - return checksum; -} - -int main() -{ - uint8_t frame_data[16]; - - // SOF - frame_data[0] = 0x5a; - frame_data[1] = 0xa5; - - // Frame len, type, ID - frame_data[2] = 0x0a; - frame_data[3] = 0xaa; - frame_data[4] = 0x01; - - // Frame payload - frame_data[5] = 0; - frame_data[6] = 1; - frame_data[7] = 2; - frame_data[8] = 3; - frame_data[9] = 4; - frame_data[10] = 5; - - // Frame count, checksum - frame_data[11] = 1; - frame_data[12] = calc_checksum(frame_data, 12); - - ScoutSerialParser parser; - parser.ParseBuffer(frame_data, 7 + 6); - - return 0; -} \ No newline at end of file diff --git a/src/tracer_sdk/CMakeLists.txt b/src/tracer_sdk/CMakeLists.txt new file mode 100755 index 0000000..aee5669 --- /dev/null +++ b/src/tracer_sdk/CMakeLists.txt @@ -0,0 +1,3 @@ +# Add source directories +add_subdirectory(tracer_base) +add_subdirectory(tracer_protocol) diff --git a/src/tracer_sdk/tracer_base/CMakeLists.txt b/src/tracer_sdk/tracer_base/CMakeLists.txt new file mode 100755 index 0000000..0f0db9a --- /dev/null +++ b/src/tracer_sdk/tracer_base/CMakeLists.txt @@ -0,0 +1,16 @@ +## Add libraries +set(TRACER_BASE_SRC + src/tracer_base.cpp +) +add_library(tracerbase STATIC ${TRACER_BASE_SRC}) +target_link_libraries(tracerbase asyncio tracer_protocol stopwatch) +target_include_directories(tracerbase PUBLIC + $ + $ + $ + PRIVATE src) + + ## Add executables +if(BUILD_TESTS) + add_subdirectory(tests) +endif() \ No newline at end of file diff --git a/src/tracer_sdk/tracer_base/include/tracer_base/tracer_base.hpp b/src/tracer_sdk/tracer_base/include/tracer_base/tracer_base.hpp new file mode 100644 index 0000000..9da51e6 --- /dev/null +++ b/src/tracer_sdk/tracer_base/include/tracer_base/tracer_base.hpp @@ -0,0 +1,109 @@ +/* + * tracer_base.hpp + * + * Created on: Apr 14, 2020 10:21 + * Description: + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#ifndef TRACER_BASE_HPP +#define TRACER_BASE_HPP + +#include +#include +#include +#include +#include + +#include "async_io/async_can.hpp" +#include "async_io/async_serial.hpp" + +#include "tracer_protocol/tracer_protocol.h" +#include "tracer_protocol/tracer_can_parser.h" + +#include "tracer_base/tracer_types.hpp" + +namespace wescore +{ +class TracerBase +{ +public: + TracerBase() = default; + ~TracerBase(); + + // do not allow copy + TracerBase(const TracerBase &tracer) = delete; + TracerBase &operator=(const TracerBase &tracer) = delete; + +public: + // connect to roboot from CAN + void Connect(std::string dev_name); + + // disconnect from roboot, only valid for serial port + void Disconnect(); + + // cmd thread runs at 100Hz (10ms) by default + void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; }; + + // motion control + void SetMotionCommand(double linear_vel, double angular_vel, + TracerMotionCmd::FaultClearFlag fault_clr_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT); + + // light control + void SetLightCommand(TracerLightCmd cmd); + void DisableLightCmdControl(); + + // get robot state + TracerState GetTracerState(); + +private: + // hardware communication interface + std::shared_ptr can_if_; + std::shared_ptr serial_if_; + + // CAN priority higher than serial if both connected + bool can_connected_ = false; + bool serial_connected_ = false; + + // serial port related variables + uint8_t tx_cmd_len_; + uint8_t tx_buffer_[TRACER_CMD_BUF_LEN]; + + // cmd/status update related variables + std::thread cmd_thread_; + std::mutex tracer_state_mutex_; + std::mutex motion_cmd_mutex_; + std::mutex light_cmd_mutex_; + + TracerState tracer_state_; + TracerMotionCmd current_motion_cmd_; + TracerLightCmd current_light_cmd_; + + int32_t cmd_thread_period_ms_ = 10; + bool cmd_thread_started_ = false; + + bool light_ctrl_enabled_ = false; + bool light_ctrl_requested_ = false; + + // internal functions + void ConfigureCANBus(const std::string &can_if_name = "can1"); + void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0", int32_t baud_rate = 115200); + + void StartCmdThread(); + void ControlLoop(int32_t period_ms); + + void SendMotionCmd(uint8_t count); + void SendLightCmd(uint8_t count); + + void ParseCANFrame(can_frame *rx_frame); + void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received); + + void NewStatusMsgReceivedCallback(const TracerMessage &msg); + +public: + static void UpdateTracerState(const TracerMessage &status_msg, TracerState &state); +}; +} // namespace wescore + +#endif /* TRACER_BASE_HPP */ diff --git a/src/tracer_sdk/tracer_base/include/tracer_base/tracer_types.hpp b/src/tracer_sdk/tracer_base/include/tracer_base/tracer_types.hpp new file mode 100644 index 0000000..43945a0 --- /dev/null +++ b/src/tracer_sdk/tracer_base/include/tracer_base/tracer_types.hpp @@ -0,0 +1,111 @@ +/* + * tracer_types.hpp + * + * Created on: Apr 14, 2020 10:22 + * Description: + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#ifndef TRACER_TYPES_HPP +#define TRACER_TYPES_HPP + +#include +#include + +namespace wescore +{ +struct TracerState +{ + enum MotorID + { + FRONT_RIGHT = 0, + FRONT_LEFT = 1, + REAR_LEFT = 2, + REAR_RIGHT = 3 + }; + + struct MotorState + { + double current = 0; // in A + double rpm = 0; + double temperature = 0; + }; + + struct LightState + { + uint8_t mode = 0; + uint8_t custom_value = 0; + }; + + // base state + uint8_t base_state = 0; + uint8_t control_mode = 0; + uint16_t fault_code = 0; + double battery_voltage = 0.0; + + // motor state + MotorState motor_states[4]; + + // light state + bool light_control_enabled = false; + LightState front_light_state; + LightState rear_light_state; + + // motion state + double linear_velocity = 0; + double angular_velocity = 0; +}; + +struct TracerMotionCmd +{ + enum class FaultClearFlag + { + NO_FAULT = 0x00, + BAT_UNDER_VOL = 0x01, + BAT_OVER_VOL = 0x02, + MOTOR1_COMM = 0x03, + MOTOR2_COMM = 0x04, + MOTOR3_COMM = 0x05, + MOTOR4_COMM = 0x06, + MOTOR_DRV_OVERHEAT = 0x07, + MOTOR_OVERCURRENT = 0x08 + }; + + TracerMotionCmd(int8_t linear = 0, int8_t angular = 0, + FaultClearFlag fault_clr_flag = FaultClearFlag::NO_FAULT) + : linear_velocity(linear), angular_velocity(angular), + fault_clear_flag(fault_clr_flag) {} + + int8_t linear_velocity; + int8_t angular_velocity; + FaultClearFlag fault_clear_flag; + + static constexpr double max_linear_velocity = 1.5; // 1.5 m/s + static constexpr double min_linear_velocity = -1.5; // -1.5 m/s + static constexpr double max_angular_velocity = 0.5235; // 0.5235 rad/s + static constexpr double min_angular_velocity = -0.5235; // -0.5235 rad/s +}; + +struct TracerLightCmd +{ + enum class LightMode + { + CONST_OFF = 0x00, + CONST_ON = 0x01, + BREATH = 0x02, + CUSTOM = 0x03 + }; + + TracerLightCmd() = default; + TracerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) : front_mode(f_mode), front_custom_value(f_value), + rear_mode(r_mode), rear_custom_value(r_value) {} + + LightMode front_mode; + uint8_t front_custom_value; + LightMode rear_mode; + uint8_t rear_custom_value; +}; +} // namespace wescore + +#endif /* TRACER_TYPES_HPP */ diff --git a/src/tracer_sdk/tracer_base/src/tracer_base.cpp b/src/tracer_sdk/tracer_base/src/tracer_base.cpp new file mode 100644 index 0000000..de1680b --- /dev/null +++ b/src/tracer_sdk/tracer_base/src/tracer_base.cpp @@ -0,0 +1,338 @@ +#include "tracer_base/tracer_base.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "stopwatch/stopwatch.h" + +namespace wescore +{ +TracerBase::~TracerBase() +{ + if (serial_connected_) + serial_if_->close(); + + if (cmd_thread_.joinable()) + cmd_thread_.join(); +} + +void TracerBase::Connect(std::string dev_name) +{ + // if (baud_rate == 0) { + ConfigureCANBus(dev_name); + // } else { + // ConfigureSerial(dev_name, baud_rate); + + // if (!serial_connected_) + // std::cerr << "ERROR: Failed to connect to serial port" << std::endl; + // } +} + +void TracerBase::Disconnect() +{ + if (serial_connected_) + { + if (serial_if_->is_open()) + serial_if_->close(); + } +} + +void TracerBase::ConfigureCANBus(const std::string &can_if_name) +{ + can_if_ = std::make_shared(can_if_name); + + can_if_->set_receive_callback(std::bind(&TracerBase::ParseCANFrame, this, std::placeholders::_1)); + + can_connected_ = true; +} + +void TracerBase::ConfigureSerial(const std::string uart_name, int32_t baud_rate) +{ + serial_if_ = std::make_shared(uart_name, baud_rate); + serial_if_->open(); + + if (serial_if_->is_open()) + serial_connected_ = true; + + serial_if_->set_receive_callback(std::bind(&TracerBase::ParseUARTBuffer, this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3)); +} + +void TracerBase::StartCmdThread() +{ + current_motion_cmd_.linear_velocity = 0; + current_motion_cmd_.angular_velocity = 0; + current_motion_cmd_.fault_clear_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT; + + cmd_thread_ = std::thread(std::bind(&TracerBase::ControlLoop, this, cmd_thread_period_ms_)); + cmd_thread_started_ = true; +} + +void TracerBase::SendMotionCmd(uint8_t count) +{ + // motion control message + TracerMessage m_msg; + m_msg.type = TracerMotionCmdMsg; + + if (can_connected_) + m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN; + else if (serial_connected_) + m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART; + + motion_cmd_mutex_.lock(); + m_msg.body.motion_cmd_msg.data.cmd.fault_clear_flag = static_cast(current_motion_cmd_.fault_clear_flag); + m_msg.body.motion_cmd_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity; + m_msg.body.motion_cmd_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity; + motion_cmd_mutex_.unlock(); + + m_msg.body.motion_cmd_msg.data.cmd.reserved0 = 0; + m_msg.body.motion_cmd_msg.data.cmd.reserved1 = 0; + m_msg.body.motion_cmd_msg.data.cmd.count = count; + + if (can_connected_) + m_msg.body.motion_cmd_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_MOTION_CMD_ID, m_msg.body.motion_cmd_msg.data.raw, 8); + // serial_connected_: checksum will be calculated later when packed into a complete serial frame + + if (can_connected_) + { + // send to can bus + can_frame m_frame; + EncodeTracerMsgToCAN(&m_msg, &m_frame); + can_if_->send_frame(m_frame); + } + else + { + // TODO + // send to serial port + // EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); + // serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); + } +} + +void TracerBase::SendLightCmd(uint8_t count) +{ + TracerMessage l_msg; + l_msg.type = TracerLightControlMsg; + + light_cmd_mutex_.lock(); + if (light_ctrl_enabled_) + { + l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL; + + l_msg.body.light_control_msg.data.cmd.front_light_mode = static_cast(current_light_cmd_.front_mode); + l_msg.body.light_control_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value; + l_msg.body.light_control_msg.data.cmd.rear_light_mode = static_cast(current_light_cmd_.rear_mode); + l_msg.body.light_control_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value; + + // std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " << l_msg.data.cmd.front_light_custom << " , " + // << l_msg.data.cmd.rear_light_mode << " , " << l_msg.data.cmd.rear_light_custom << std::endl; + // std::cout << "light cmd generated" << std::endl; + } + else + { + l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_DISABLE_CTRL; + + l_msg.body.light_control_msg.data.cmd.front_light_mode = LIGHT_MODE_CONST_OFF; + l_msg.body.light_control_msg.data.cmd.front_light_custom = 0; + l_msg.body.light_control_msg.data.cmd.rear_light_mode = LIGHT_MODE_CONST_OFF; + l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0; + } + light_ctrl_requested_ = false; + light_cmd_mutex_.unlock(); + + l_msg.body.light_control_msg.data.cmd.reserved0 = 0; + l_msg.body.light_control_msg.data.cmd.count = count; + + if (can_connected_) + l_msg.body.light_control_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8); + // serial_connected_: checksum will be calculated later when packed into a complete serial frame + + if (can_connected_) + { + // send to can bus + can_frame l_frame; + EncodeTracerMsgToCAN(&l_msg, &l_frame); + + can_if_->send_frame(l_frame); + } + // else + // { + // // send to serial port + // EncodeTracerMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_); + // serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); + // } + + // std::cout << "cmd: " << static_cast(l_msg.data.cmd.front_light_mode) << " , " << static_cast(l_msg.data.cmd.front_light_custom) << " , " + // << static_cast(l_msg.data.cmd.rear_light_mode) << " , " << static_cast(l_msg.data.cmd.rear_light_custom) << std::endl; + // std::cout << "can: "; + // for (int i = 0; i < 8; ++i) + // std::cout << static_cast(l_frame.data[i]) << " "; + // std::cout << "uart: "; + // for (int i = 0; i < tx_cmd_len_; ++i) + // std::cout << static_cast(tx_buffer_[i]) << " "; + // std::cout << std::endl; +} + +void TracerBase::ControlLoop(int32_t period_ms) +{ + StopWatch ctrl_sw; + uint8_t cmd_count = 0; + uint8_t light_cmd_count = 0; + while (true) + { + ctrl_sw.tic(); + + // motion control message + SendMotionCmd(cmd_count++); + + // check if there is request for light control + if (light_ctrl_requested_) + SendLightCmd(light_cmd_count++); + + ctrl_sw.sleep_until_ms(period_ms); + // std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << std::endl; + } +} + +TracerState TracerBase::GetTracerState() +{ + std::lock_guard guard(tracer_state_mutex_); + return tracer_state_; +} + +void TracerBase::SetMotionCommand(double linear_vel, double angular_vel, TracerMotionCmd::FaultClearFlag fault_clr_flag) +{ + // make sure cmd thread is started before attempting to send commands + if (!cmd_thread_started_) + StartCmdThread(); + + if (linear_vel < TracerMotionCmd::min_linear_velocity) + linear_vel = TracerMotionCmd::min_linear_velocity; + if (linear_vel > TracerMotionCmd::max_linear_velocity) + linear_vel = TracerMotionCmd::max_linear_velocity; + if (angular_vel < TracerMotionCmd::min_angular_velocity) + angular_vel = TracerMotionCmd::min_angular_velocity; + if (angular_vel > TracerMotionCmd::max_angular_velocity) + angular_vel = TracerMotionCmd::max_angular_velocity; + + std::lock_guard guard(motion_cmd_mutex_); + current_motion_cmd_.linear_velocity = static_cast(linear_vel / TracerMotionCmd::max_linear_velocity * 100.0); + current_motion_cmd_.angular_velocity = static_cast(angular_vel / TracerMotionCmd::max_angular_velocity * 100.0); + current_motion_cmd_.fault_clear_flag = fault_clr_flag; +} + +void TracerBase::SetLightCommand(TracerLightCmd cmd) +{ + if (!cmd_thread_started_) + StartCmdThread(); + + std::lock_guard guard(light_cmd_mutex_); + current_light_cmd_ = cmd; + light_ctrl_enabled_ = true; + light_ctrl_requested_ = true; +} + +void TracerBase::DisableLightCmdControl() +{ + std::lock_guard guard(light_cmd_mutex_); + light_ctrl_enabled_ = false; + light_ctrl_requested_ = true; +} + +void TracerBase::ParseCANFrame(can_frame *rx_frame) +{ + // validate checksum, discard frame if fails + if (!rx_frame->data[7] == CalcTracerCANChecksum(rx_frame->can_id, rx_frame->data, rx_frame->can_dlc)) + { + std::cerr << "ERROR: checksum mismatch, discard frame with id " << rx_frame->can_id << std::endl; + return; + } + + // otherwise, update robot state with new frame + TracerMessage status_msg; + DecodeTracerMsgFromCAN(rx_frame, &status_msg); + NewStatusMsgReceivedCallback(status_msg); +} + +void TracerBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received) +{ + // std::cout << "bytes received from serial: " << bytes_received << std::endl; + // serial_parser_.PrintStatistics(); + // serial_parser_.ParseBuffer(buf, bytes_received); + + // TODO + // TracerMessage status_msg; + // for (int i = 0; i < bytes_received; ++i) + // { + // if (DecodeTracerMsgFromUART(buf[i], &status_msg)) + // NewStatusMsgReceivedCallback(status_msg); + // } +} + +void TracerBase::NewStatusMsgReceivedCallback(const TracerMessage &msg) +{ + // std::cout << "new status msg received" << std::endl; + std::lock_guard guard(tracer_state_mutex_); + UpdateTracerState(msg, tracer_state_); +} + +void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState &state) +{ + switch (status_msg.type) + { + case TracerMotionStatusMsg: + { + // std::cout << "motion control feedback received" << std::endl; + const MotionStatusMessage &msg = status_msg.body.motion_status_msg; + state.linear_velocity = static_cast(static_cast(msg.data.status.linear_velocity.low_byte) | static_cast(msg.data.status.linear_velocity.high_byte) << 8) / 1000.0; + state.angular_velocity = static_cast(static_cast(msg.data.status.angular_velocity.low_byte) | static_cast(msg.data.status.angular_velocity.high_byte) << 8) / 1000.0; + break; + } + case TracerLightStatusMsg: + { + // std::cout << "light control feedback received" << std::endl; + const LightStatusMessage &msg = status_msg.body.light_status_msg; + if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL) + state.light_control_enabled = false; + else + state.light_control_enabled = true; + state.front_light_state.mode = msg.data.status.front_light_mode; + state.front_light_state.custom_value = msg.data.status.front_light_custom; + state.rear_light_state.mode = msg.data.status.rear_light_mode; + state.rear_light_state.custom_value = msg.data.status.rear_light_custom; + break; + } + case TracerSystemStatusMsg: + { + // std::cout << "system status feedback received" << std::endl; + const SystemStatusMessage &msg = status_msg.body.system_status_msg; + state.control_mode = msg.data.status.control_mode; + state.base_state = msg.data.status.base_state; + state.battery_voltage = (static_cast(msg.data.status.battery_voltage.low_byte) | static_cast(msg.data.status.battery_voltage.high_byte) << 8) / 10.0; + state.fault_code = (static_cast(msg.data.status.fault_code.low_byte) | static_cast(msg.data.status.fault_code.high_byte) << 8); + break; + } + case TracerMotorDriverStatusMsg: + { + // std::cout << "motor 1 driver feedback received" << std::endl; + const MotorDriverStatusMessage &msg = status_msg.body.motor_driver_status_msg; + for (int i = 0; i < 4; ++i) + { + state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].current = (static_cast(msg.data.status.current.low_byte) | static_cast(msg.data.status.current.high_byte) << 8) / 10.0; + state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].rpm = static_cast(static_cast(msg.data.status.rpm.low_byte) | static_cast(msg.data.status.rpm.high_byte) << 8); + state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].temperature = msg.data.status.temperature; + } + break; + } + } +} +} // namespace wescore diff --git a/src/tracer_sdk/tracer_base/tests/CMakeLists.txt b/src/tracer_sdk/tracer_base/tests/CMakeLists.txt new file mode 100644 index 0000000..2dd735c --- /dev/null +++ b/src/tracer_sdk/tracer_base/tests/CMakeLists.txt @@ -0,0 +1,16 @@ +# Dependency libraries +#find_package(LIBRARY_NAME REQUIRED) + +# tests +add_executable(test_scout_base test_scout_base.cpp) +target_link_libraries(test_scout_base scoutbase) + +# add_executable(test_scout_serial test_scout_serial.cpp) +# target_link_libraries(test_scout_serial scoutbase) + +# add_executable(test_can_msg test_can_msg.cpp) +# target_link_libraries(test_can_msg scoutbase) + +# add_executable(test_serial_parser test_serial_parser.cpp) +# target_link_libraries(test_serial_parser scoutbase) + diff --git a/src/hunter_sdk/hunter_base/tests/test_scout_base.cpp b/src/tracer_sdk/tracer_base/tests/test_tracer_base.cpp similarity index 100% rename from src/hunter_sdk/hunter_base/tests/test_scout_base.cpp rename to src/tracer_sdk/tracer_base/tests/test_tracer_base.cpp diff --git a/src/tracer_sdk/tracer_protocol/CMakeLists.txt b/src/tracer_sdk/tracer_protocol/CMakeLists.txt new file mode 100755 index 0000000..4cd60f3 --- /dev/null +++ b/src/tracer_sdk/tracer_protocol/CMakeLists.txt @@ -0,0 +1,9 @@ +## Add libraries +set(TRACER_PROTOCOL_SRC + src/tracer_can_parser.c +) +add_library(tracer_protocol STATIC ${TRACER_PROTOCOL_SRC}) +target_include_directories(tracer_protocol PUBLIC + $ + $ + PRIVATE src) \ No newline at end of file diff --git a/src/tracer_sdk/tracer_protocol/include/tracer_protocol/tracer_can_parser.h b/src/tracer_sdk/tracer_protocol/include/tracer_protocol/tracer_can_parser.h new file mode 100644 index 0000000..ecc8401 --- /dev/null +++ b/src/tracer_sdk/tracer_protocol/include/tracer_protocol/tracer_can_parser.h @@ -0,0 +1,42 @@ +/* + * tracer_can_parser.h + * + * Created on: Apr 14, 2020 10:52 + * Description: + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#ifndef TRACER_CAN_PARSER_H +#define TRACER_CAN_PARSER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#include "tracer_protocol/tracer_protocol.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 DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg); +void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame); + +uint8_t CalcTracerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc); + +#ifdef __cplusplus +} +#endif + +#endif /* TRACER_CAN_PARSER_H */ diff --git a/src/tracer_sdk/tracer_protocol/include/tracer_protocol/tracer_protocol.h b/src/tracer_sdk/tracer_protocol/include/tracer_protocol/tracer_protocol.h new file mode 100644 index 0000000..de5e0bf --- /dev/null +++ b/src/tracer_sdk/tracer_protocol/include/tracer_protocol/tracer_protocol.h @@ -0,0 +1,259 @@ +/* + * tracer_protocol.h + * + * Created on: Apr 14, 2020 10:34 + * Description: + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#ifndef TRACER_PROTOCOL_H +#define TRACER_PROTOCOL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define TRACER_CMD_BUF_LEN 32 +#define TRACER_STATUS_BUF_LEN 32 +#define TRACER_FRAME_SIZE 13 + +#define TRACER_MOTOR1_ID ((uint8_t)0x00) +#define TRACER_MOTOR2_ID ((uint8_t)0x01) + +// CAN Definitions +#define CAN_MSG_MOTION_CMD_ID ((uint32_t)0x130) +#define CAN_MSG_MOTION_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)0x201) +#define CAN_MSG_MOTOR2_DRIVER_STATUS_ID ((uint32_t)0x202) + +/*--------------------- 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_FRONT_STEER_ENCODER_F ((uint16_t)0x0200) +#define FAULT_RC_SIGNAL_LOSS ((uint16_t)0x0400) +#define FAULT_HIGH_BYTE_RESERVED1 ((uint16_t)0x0800) +#define FAULT_HIGH_BYTE_RESERVED2 ((uint16_t)0x1000) +#define FAULT_HIGH_BYTE_RESERVED3 ((uint16_t)0x2000) +#define FAULT_HIGH_BYTE_RESERVED4 ((uint16_t)0x4000) +#define FAULT_HIGH_BYTE_RESERVED5 ((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_RESERVED1 ((uint16_t)0x0010) +#define FAULT_RESERVED2 ((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; +} MotionCmdMessage; + +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 +{ + TracerMsgNone = 0x00, + // status messages + TracerMotionStatusMsg = 0x01, + TracerLightStatusMsg = 0x02, + TracerSystemStatusMsg = 0x03, + TracerMotorDriverStatusMsg = 0x04, + // control messages + TracerMotionCmdMsg = 0x21, + TracerLightControlMsg = 0x22 +} TracerMsgType; + +typedef struct +{ + TracerMsgType type; + union { + // status messages + MotionStatusMessage motion_status_msg; + LightStatusMessage light_status_msg; + SystemStatusMessage system_status_msg; + MotorDriverStatusMessage motor_driver_status_msg; + // control messages + MotionCmdMessage motion_cmd_msg; + LightControlMessage light_control_msg; + } body; +} TracerMessage; + +#pragma pack(pop) + +#ifdef __cplusplus +} +#endif + +#endif /* TRACER_PROTOCOL_H */ diff --git a/src/tracer_sdk/tracer_protocol/src/tracer_can_parser.c b/src/tracer_sdk/tracer_protocol/src/tracer_can_parser.c new file mode 100644 index 0000000..1f9386f --- /dev/null +++ b/src/tracer_sdk/tracer_protocol/src/tracer_can_parser.c @@ -0,0 +1,118 @@ +/* + * tracer_can_parser.c + * + * Created on: Apr 14, 2020 10:35 + * Description: + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#include "tracer_protocol/tracer_can_parser.h" + +#include "string.h" + +static void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame); + +bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg) +{ + msg->type = TracerMsgNone; + + switch (rx_frame->can_id) + { + // in the current implementation, both MsgType and can_frame include 8 * uint8_t + case CAN_MSG_MOTION_STATUS_ID: + { + msg->type = TracerMotionStatusMsg; + memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + case CAN_MSG_SYSTEM_STATUS_STATUS_ID: + { + msg->type = TracerSystemStatusMsg; + 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 = TracerMotorDriverStatusMsg; + msg->body.motor_driver_status_msg.motor_id = TRACER_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 = TracerMotorDriverStatusMsg; + msg->body.motor_driver_status_msg.motor_id = TRACER_MOTOR2_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_CMD_ID: + { + msg->type = TracerMotionCmdMsg; + memcpy(msg->body.motion_cmd_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t)); + break; + } + default: + break; + } + + return true; +} + +void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame) +{ + switch (msg->type) + { + // in the current implementation, both MsgType and can_frame include 8 * uint8_t + case TracerMotionStatusMsg: + { + tx_frame->can_id = CAN_MSG_MOTION_STATUS_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc); + break; + } + case TracerSystemStatusMsg: + { + 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 TracerMotorDriverStatusMsg: + { + if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR1_ID) + tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID; + else if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR2_ID) + tx_frame->can_id = CAN_MSG_MOTOR2_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 TracerMotionCmdMsg: + { + EncodeTracerMotionControlMsgToCAN(&(msg->body.motion_cmd_msg), tx_frame); + break; + } + default: + break; + } + tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); +} + +void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame) +{ + tx_frame->can_id = CAN_MSG_MOTION_CMD_ID; + tx_frame->can_dlc = 8; + memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc); + tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); +} + +uint8_t CalcTracerCANChecksum(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