mirrors/scout_ros
1
0
Fork 0
mirror of https://github.com/westonrobot/scout_ros.git synced 2026-01-12 03:15:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

upgraded scout_sdk

Browse Source
This commit is contained in:
Ruixiang Du
2019-09-17 02:02:49 -04:00
parent ccccce9d40
commit 2ad6f201d2
86 changed files with 2568 additions and 1389 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
scout_base/CMakeLists.txt
View File

@@ -24,7 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package(
INCLUDE_DIRS include
LIBRARIES scout_base
LIBRARIES scoutbase
CATKIN_DEPENDS hardware_interface scout_msgs roscpp sensor_msgs
# DEPENDS Boost
)

2
scout_base/include/scout_base/scout_messenger.hpp
View File

@@ -17,7 +17,7 @@
#include <tf/transform_broadcaster.h>
#include "scout_msgs/ScoutLightCmd.h"
#include "scout/scout_base.hpp"
#include "scout_base/scout_base.hpp"
namespace wescore
{

9
scout_base/src/scout_base_node.cpp
View File

@@ -6,7 +6,7 @@
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include "scout/scout_base.hpp"
#include "scout_base/scout_base.hpp"
#include "scout_base/scout_messenger.hpp"
using namespace wescore;
@@ -28,17 +28,16 @@ int main(int argc, char **argv)
private_node.param<bool>("simulated_robot", messenger.simulated_robot_, false);
// connect to scout and setup ROS subscription
robot.ConnectCANBus(scout_can_port);
robot.StartCmdThread(10);
robot.Connect(scout_can_port);
messenger.SetupSubscription();
// publish robot state at 20Hz while listening to twist commands
ros::Rate rt(20); // 20Hz
ros::Rate rate_20hz(20); // 20Hz
while (true)
{
messenger.PublishStateToROS();
ros::spinOnce();
rt.sleep();
rate_20hz.sleep();
}
return 0;

11
scout_base/src/scout_sdk/CHANGELOG.md
View File

@@ -1,5 +1,16 @@
# Changelog for scout_sdk
## 0.4 (2019-09-15)
------------------
* Unified implementation of UART/CAN for firmware and SDK
* Improvements of code organization
* Contributors: Ruixiang Du
## 0.3 (2019-08-02)
------------------
* Added full UART support
* Contributors: Ruixiang Du
## 0.2 (2019-06-14)
------------------
* Deprecated initial serial interface support (new one under development)

34
scout_base/src/scout_sdk/README.md
View File

@@ -4,32 +4,25 @@ Copyright (c) 2019 [WestonRobot](https://www.westonrobot.com/)
## Introduction
This software packages provides a C++ interface to communicate with the Scout mobile base, for sending commands to the robot and acquiring latest robot state.
This software packages provides a C++ interface to communicate with the Scout mobile base, for sending commands to the robot and acquiring the latest robot state. The SDK works on both x86 and ARM platforms.
Generally, you only need to instantiate an object of "class ScoutBase", then use the object to programmatically control the robot. Internally, class ScoutBase manages two background threads, one to process CAN messages of the robot state and accordingly update state variables in the ScoutState data structre, and the other to maintain a 50Hz loop and send latest command to the robot base. User can iteratively perform tasks in the main thread and check the robot state or set control commands.
Generally, you only need to instantiate an object of "class ScoutBase", then use the object to programmatically control the robot. Internally, class ScoutBase manages two background threads, one to process CAN/UART messages of the robot state and accordingly update state variables in the ScoutState data structure, and the other to maintain a 50Hz loop and send the latest command to the robot base. User can iteratively perform tasks in the main thread and check the robot state or set control commands.
Refer to "src/demo/demo_scout_can.cpp" for an example.
Refer to "src/apps/scout_demo" for an example.
## Package Structure
* demo: demo to illustrate how to use the SDK
* monitor: a TUI application to monitor states of Scout
* sdk_core/scout_base: interface to send command to robot and receive robot state
* sdk_core/async_io: manages raw data communication with robot
* apps: demo to illustrate how to use the SDK, scout_monitor is a TUI application to monitor states of Scout
* comm/async_io: manages raw data communication with robot
* comm/scout_protocol: encoding and decoding of Scout UART/CAN protocols
* scout_base: interface to send command to robot and receive robot state
* third_party
- asio: asynchronous IO management (serial and CAN)
- googletest: for unit tests only (not required otherwise)
## Hardware Interface
## Setup CAN-To-USB adapter
* CAN: full support
* RS-232: under development
A easy and low-cost option to use the CAN interface would be using a Raspberry Pi or Beaglebone board with CAN Hat/Cape. The SDK can compile on both x86 and ARM platforms. Then you can use whatever interface you prefer (serial, USB, Ethernet etc.) for the communication between the single-board computer and your main PC.
* Setup CAN-To-USB adapter
The intructions work for stm32f0-based adapter with [candleLight](https://github.com/HubertD/candleLight_fw) firmware on a host computer running Linux. (Refer to limitations listed at the bottom for more details.)
The instructions work for stm32f0-based adapter with [candleLight](https://github.com/HubertD/candleLight_fw) firmware on a host computer running Linux. (Refer to limitations listed at the bottom for more details.)
1. Enable gs_usb kernel module
```
@@ -55,6 +48,8 @@ The intructions work for stm32f0-based adapter with [candleLight](https://github
$ cansend can0 001#1122334455667788
```
Two scripts inside the "./scripts" folder are provided for easy setup. You can run "./setup_can2usb.bash" for the first-time setup and run "./bringup_can2usb.bash" to bring up the device each time you unplug and re-plug the adapter.
## Build SDK
Install compile tools
@@ -63,7 +58,7 @@ Install compile tools
$ sudo apt install build-essential cmake
```
If you want to build the monitor, install libncurses
If you want to build the TUI monitor tool, install libncurses
```
$ sudo apt install libncurses5-dev
@@ -93,11 +88,12 @@ $ make
device interrupt 43
```
If you use a CAN-to-USB adapter, make sure it supports slcan or can be brought up as a native CAN device (for example, CANable https://www.canable.io/). Some adapters may use a custom-defined protocol and appear as a serial device in Linux. In such case, you will have to translate the byte stream between CAN and UART by yourself. It would be difficult for us to provide support for them since not all manufactures define this protocol in the same way.
If you use your own CAN-to-USB adapter, make sure it supports slcan or can be brought up as a native CAN device (for example, CANable https://www.canable.io/). Some adapters may use a custom-defined protocol and appear as a serial device in Linux. In such a case, you will have to translate the byte stream between CAN and UART by yourself. It would be difficult for us to provide support for them since not all manufacturers define this protocol in the same way.
<!--
2. Release v0.1 of this SDK provided a serial interface to talk with the robot. Front/rear light on the robot cannot be controlled and only a small subset of all robot states can be acquired through that interface. Full support of the serial interface is still under development and requires additional work on both the SDK and firmware sides.
-->
## Reference
* [Candlelight firmware](https://wiki.linklayer.com/index.php/CandleLightFirmware)
* [CAN command reference in Linux](https://wiki.rdu.im/_pages/Notes/Embedded-System/Linux/can-bus-in-linux.html)

0
scout_base/src/scout_sdk/docs/SCOUT_UserManual_v1.2.8_CN.pdf → scout_base/src/scout_sdk/docs/misc/SCOUT_UserManual_v1.2.8_CN.pdf
View File

0
scout_base/src/scout_sdk/docs/beaglebone_black_can_cape.md → scout_base/src/scout_sdk/docs/misc/beaglebone_black_can_cape.md
View File

0
scout_base/src/scout_sdk/docs/beaglebone_ros_install.md → scout_base/src/scout_sdk/docs/misc/beaglebone_ros_install.md
View File

0
scout_base/src/scout_sdk/docs/can_msg.txt → scout_base/src/scout_sdk/docs/misc/can_msg.txt
View File

0
scout_base/src/scout_sdk/docs/check_endianness.md → scout_base/src/scout_sdk/docs/misc/check_endianness.md
View File

0
scout_base/src/scout_sdk/docs/commands.md → scout_base/src/scout_sdk/docs/misc/commands.md
View File

0
scout_base/src/scout_sdk/docs/jetson_tx2_can.md → scout_base/src/scout_sdk/docs/misc/jetson_tx2_can.md
View File

0
scout_base/src/scout_sdk/docs/release_checklist.md → scout_base/src/scout_sdk/docs/misc/release_checklist.md
View File

0
scout_base/src/scout_sdk/docs/scout_interface.png → scout_base/src/scout_sdk/docs/misc/scout_interface.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 53 KiB

After

Width:  |  Height:  |  Size: 53 KiB

0
scout_base/src/scout_sdk/docs/scout_monitor_tool.gif → scout_base/src/scout_sdk/docs/misc/scout_monitor_tool.gif
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 540 KiB

After

Width:  |  Height:  |  Size: 540 KiB

0
scout_base/src/scout_sdk/docs/test_can_msg.txt → scout_base/src/scout_sdk/docs/misc/test_can_msg.txt
View File

4
scout_base/src/scout_sdk/scripts/bringup_can2usb.bash Executable file
View File

@@ -0,0 +1,4 @@
#!/bin/bash
# bring up can interface
sudo ip link set can0 up type can bitrate 500000

6
scout_base/src/scout_sdk/scripts/remote_ws/beaglebone_black.code-workspace
View File

@@ -1,6 +0,0 @@
{
"folders": [{
"uri": "sftp://rdu:rdu3317*@192.168.7.2/home/rdu/Workspace/scout_sdk",
"name": "Scout SDK Workspace"
}]
}

10
scout_base/src/scout_sdk/scripts/setup_can2usb.bash Executable file
View File

@@ -0,0 +1,10 @@
#!/bin/bash
# enable kernel module: gs_usb
sudo modprobe gs_usb
# bring up can interface
sudo ip link set can0 up type can bitrate 500000
# install can utils
sudo apt install -y can-utils

9
scout_base/src/scout_sdk/src/CMakeLists.txt
View File

@@ -1,8 +1,9 @@
# Add source directories
add_subdirectory(demo)
add_subdirectory(sdk_core)
add_subdirectory(apps)
add_subdirectory(comm)
add_subdirectory(scout_base)
add_subdirectory(third_party)
if(BUILD_MONITOR)
add_subdirectory(monitor)
if(BUILD_TESTS)
add_subdirectory(unit_tests)
endif()

16
scout_base/src/scout_sdk/src/apps/CMakeLists.txt Executable file
View File

@@ -0,0 +1,16 @@
# Dependency libraries
#find_package(LIBRARY_NAME REQUIRED)
# tests
add_executable(app_scout_demo scout_demo/scout_demo.cpp)
target_link_libraries(app_scout_demo scoutbase)
# add_executable(demo_scout_can demo_scout_can.cpp)
# target_link_libraries(demo_scout_can scoutbase)
# add_executable(demo_scout_serial demo_scout_serial.cpp)
# target_link_libraries(demo_scout_serial scoutbase)
if(BUILD_MONITOR)
add_subdirectory(scout_monitor)
endif()

116
scout_base/src/scout_sdk/src/apps/scout_demo/scout_demo.cpp Normal file
View File

@@ -0,0 +1,116 @@
/*
* demo_scout_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "scout_base/scout_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 if (argc == 3)
{
device_name = {argv[1]};
baud_rate = std::stol(argv[2]);
std::cout << "Specified serial: " << device_name << "@" << baud_rate << std::endl;
}
else
{
std::cout << "Usage: app_scout_monitor <interface>" << std::endl
<< "Example 1: ./app_scout_demo can0" << std::endl
<< "Example 2: ./app_scout_demo /dev/ttyUSB0 115200" << std::endl;
return -1;
}
ScoutBase scout;
scout.Connect(device_name, baud_rate);
// light control
std::cout << "Light: const off" << std::endl;
scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_OFF, 0, ScoutLightCmd::LightMode::CONST_OFF, 0});
sleep(3);
std::cout << "Light: const on" << std::endl;
scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0});
sleep(3);
std::cout << "Light: breath" << std::endl;
scout.SetLightCommand({ScoutLightCmd::LightMode::BREATH, 0, ScoutLightCmd::LightMode::BREATH, 0});
sleep(3);
std::cout << "Light: custom 90-80" << std::endl;
scout.SetLightCommand({ScoutLightCmd::LightMode::CUSTOM, 90, ScoutLightCmd::LightMode::CUSTOM, 80});
sleep(3);
std::cout << "Light: diabled cmd control" << std::endl;
scout.DisableLightCmdControl();
int count = 0;
while (true)
{
// motion control
if (count < 5)
{
std::cout << "Motor: 0.2, 0" << std::endl;
scout.SetMotionCommand(0.2, 0.0);
}
else if (count < 10)
{
std::cout << "Motor: 0.8, 0" << std::endl;
scout.SetMotionCommand(0.8, 0.0);
}
else if (count < 15)
{
std::cout << "Motor: 1.5, 0" << std::endl;
scout.SetMotionCommand(1.5, 0.0);
}
else if (count < 20)
{
std::cout << "Motor: 1.0, 0.5" << std::endl;
scout.SetMotionCommand(1.0, 0.5);
}
else if (count < 25)
{
std::cout << "Motor: 0.0, 0" << std::endl;
scout.SetMotionCommand(0.0, 0.0);
}
else if (count < 30)
{
std::cout << "Motor: -0.5, 0" << std::endl;
scout.SetMotionCommand(-0.5, 0.0);
}
else if (count < 35)
{
std::cout << "Motor: -1.0, -0.5" << std::endl;
scout.SetMotionCommand(-1.0, -0.5);
}
else if (count < 40)
{
std::cout << "Motor: 0.0, 0, Light: breath" << std::endl;
scout.SetMotionCommand(0.0, 0.0);
scout.SetLightCommand({ScoutLightCmd::LightMode::BREATH, 0, ScoutLightCmd::LightMode::BREATH, 0});
}
auto state = scout.GetScoutState();
std::cout << "-------------------------------" << std::endl;
std::cout << "count: " << count << std::endl;
std::cout << "control mode: " << static_cast<int>(state.control_mode) << " , base state: " << static_cast<int>(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;
}

0
scout_base/src/scout_sdk/src/monitor/CMakeLists.txt → scout_base/src/scout_sdk/src/apps/scout_monitor/CMakeLists.txt
View File

3
scout_base/src/scout_sdk/src/apps/scout_monitor/app/CMakeLists.txt Normal file
View File

@@ -0,0 +1,3 @@
# Add executables
add_executable(app_scout_monitor app_scout_monitor.cpp)
target_link_libraries(app_scout_monitor monitor)

40
scout_base/src/scout_sdk/src/apps/scout_monitor/app/app_scout_monitor.cpp Normal file
View File

@@ -0,0 +1,40 @@
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <string>
#include <iostream>
#include "monitor/scout_monitor.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 if (argc == 3)
{
device_name = {argv[1]};
baud_rate = std::stol(argv[2]);
std::cout << "Specified serial: " << device_name << "@" << baud_rate << std::endl;
}
else
{
std::cout << "Usage: app_scout_monitor <interface>" << std::endl
<< "Example 1: ./app_scout_monitor can1" << std::endl
<< "Example 2: ./app_scout_monitor /dev/ttyUSB0 115200" << std::endl;
return -1;
}
ScoutMonitor monitor;
monitor.Run(device_name, baud_rate);
return 0;
}

0
scout_base/src/scout_sdk/src/monitor/color_demo.png → scout_base/src/scout_sdk/src/apps/scout_monitor/color_demo.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 85 KiB

After

Width:  |  Height:  |  Size: 85 KiB

0
scout_base/src/scout_sdk/src/monitor/include/monitor/ncolors.hpp → scout_base/src/scout_sdk/src/apps/scout_monitor/include/monitor/ncolors.hpp
View File

0
scout_base/src/scout_sdk/src/monitor/include/monitor/nshapes.hpp → scout_base/src/scout_sdk/src/apps/scout_monitor/include/monitor/nshapes.hpp
View File

5
scout_base/src/scout_sdk/src/monitor/include/monitor/scout_monitor.hpp → scout_base/src/scout_sdk/src/apps/scout_monitor/include/monitor/scout_monitor.hpp
View File

@@ -10,8 +10,7 @@
#ifndef SCOUT_MONITOR_HPP
#define SCOUT_MONITOR_HPP
#include "scout/scout_base.hpp"
#include "scout/scout_state.hpp"
#include "scout_base/scout_base.hpp"
#include <ncurses.h>
@@ -23,7 +22,7 @@ public:
ScoutMonitor();
~ScoutMonitor();
void Run(std::string device_name = "");
void Run(std::string device_name = "", int32_t baud_rate = 0);
void Terminate() { keep_running_ = false; }
private:

0
scout_base/src/scout_sdk/src/monitor/src/ncolors.cpp → scout_base/src/scout_sdk/src/apps/scout_monitor/src/ncolors.cpp
View File

0
scout_base/src/scout_sdk/src/monitor/src/nshapes.cpp → scout_base/src/scout_sdk/src/apps/scout_monitor/src/nshapes.cpp
View File

109
scout_base/src/scout_sdk/src/monitor/src/scout_monitor.cpp → scout_base/src/scout_sdk/src/apps/scout_monitor/src/scout_monitor.cpp
View File

@@ -35,8 +35,6 @@
#include <ratio>
#include <thread>
#include "scout/scout_can_protocol.h"
#include "scout/scout_command.hpp"
#include "monitor/nshapes.hpp"
#include "monitor/ncolors.hpp"
@@ -133,6 +131,9 @@ ScoutMonitor::ScoutMonitor()
body_info_win_ = newwin(bi_win_sy_, bi_win_sx_, bi_origin_y_, bi_origin_x_);
system_info_win_ = newwin(si_win_sy_, si_win_sx_, si_origin_y_, si_origin_x_);
scout_state_.linear_velocity = 0;
scout_state_.angular_velocity = 0;
NColors::InitColors();
}
@@ -157,26 +158,26 @@ void ScoutMonitor::UpdateAll()
void ScoutMonitor::SetTestStateData()
{
scout_state_.base_state = BASE_NORMAL;
scout_state_.base_state = BASE_STATE_NORMAL;
scout_state_.battery_voltage = 28.5;
scout_state_.linear_velocity = 1.234;
scout_state_.angular_velocity = -0.6853;
// scout_state_.fault_code |= MOTOR_DRV_OVERHEAT_W;
// scout_state_.fault_code |= MOTOR_OVERCURRENT_W;
// scout_state_.fault_code |= MOTOR_DRV_OVERHEAT_F;
// scout_state_.fault_code |= MOTOR_OVERCURRENT_F;
// scout_state_.fault_code |= BAT_UNDER_VOL_W;
// scout_state_.fault_code |= BAT_UNDER_VOL_F;
// scout_state_.fault_code |= FAULT_MOTOR_DRV_OVERHEAT_W;
// scout_state_.fault_code |= FAULT_MOTOR_OVERCURRENT_W;
// scout_state_.fault_code |= FAULT_MOTOR_DRV_OVERHEAT_F;
// scout_state_.fault_code |= FAULT_MOTOR_OVERCURRENT_F;
// scout_state_.fault_code |= FAULT_BAT_UNDER_VOL_W;
// scout_state_.fault_code |= FAULT_BAT_UNDER_VOL_F;
scout_state_.fault_code = 0x0000;
// scout_state_.fault_code = 0xffff;
// scout_state_.front_light_state.mode = CONST_ON;
scout_state_.front_light_state.mode = CUSTOM;
scout_state_.front_light_state.mode = LIGHT_MODE_CUSTOM;
scout_state_.front_light_state.custom_value = 50;
scout_state_.rear_light_state.mode = CONST_ON;
scout_state_.rear_light_state.mode = LIGHT_MODE_CONST_ON;
scout_state_.motor_states[0].current = 10.1;
scout_state_.motor_states[0].rpm = 2000;
@@ -195,7 +196,7 @@ void ScoutMonitor::SetTestStateData()
scout_state_.motor_states[3].temperature = 35;
}
void ScoutMonitor::Run(std::string device_name)
void ScoutMonitor::Run(std::string device_name, int32_t baud_rate)
{
if (device_name != "")
test_mode_ = false;
@@ -203,7 +204,7 @@ void ScoutMonitor::Run(std::string device_name)
if (test_mode_)
SetTestStateData();
else
scout_base_.ConnectCANBus(device_name);
scout_base_.Connect(device_name, baud_rate);
StopWatch sw;
while (keep_running_)
@@ -377,19 +378,19 @@ void ScoutMonitor::ShowVehicleState(int y, int x)
// show vehicle lights
std::string front_mode_str = "Mode: ";
if (scout_state_.front_light_state.mode == CONST_ON)
if (scout_state_.front_light_state.mode == LIGHT_MODE_CONST_ON)
front_mode_str += "ON";
else if (scout_state_.front_light_state.mode == CONST_OFF)
else if (scout_state_.front_light_state.mode == LIGHT_MODE_CONST_OFF)
front_mode_str += "OFF";
else if (scout_state_.front_light_state.mode == BREATH)
else if (scout_state_.front_light_state.mode == LIGHT_MODE_BREATH)
front_mode_str += "BREATH";
else if (scout_state_.front_light_state.mode == CUSTOM)
else if (scout_state_.front_light_state.mode == LIGHT_MODE_CUSTOM)
front_mode_str += "CUSTOM";
mvwprintw(body_info_win_, linear_axis_tip_y - 4, angular_axis_origin_x - 13, front_mode_str.c_str());
std::string front_custom_str = "Custom: " + ConvertFloatToString(scout_state_.front_light_state.custom_value, 0);
mvwprintw(body_info_win_, linear_axis_tip_y - 4, angular_axis_origin_x + 3, front_custom_str.c_str());
if (scout_state_.front_light_state.mode != CONST_OFF &&
!(scout_state_.front_light_state.mode == CUSTOM && scout_state_.front_light_state.custom_value == 0))
if (scout_state_.front_light_state.mode != LIGHT_MODE_CONST_OFF &&
!(scout_state_.front_light_state.mode == LIGHT_MODE_CUSTOM && scout_state_.front_light_state.custom_value == 0))
{
NColors::WSetColor(body_info_win_, NColors::BRIGHT_YELLOW);
for (int i = angular_axis_origin_x - 5; i < angular_axis_origin_x - 1; ++i)
@@ -401,19 +402,19 @@ void ScoutMonitor::ShowVehicleState(int y, int x)
}
std::string rear_mode_str = "Mode: ";
if (scout_state_.rear_light_state.mode == CONST_ON)
if (scout_state_.rear_light_state.mode == LIGHT_MODE_CONST_ON)
rear_mode_str += "ON";
else if (scout_state_.rear_light_state.mode == CONST_OFF)
else if (scout_state_.rear_light_state.mode == LIGHT_MODE_CONST_OFF)
rear_mode_str += "OFF";
else if (scout_state_.rear_light_state.mode == BREATH)
else if (scout_state_.rear_light_state.mode == LIGHT_MODE_BREATH)
rear_mode_str += "BREATH";
else if (scout_state_.rear_light_state.mode == CUSTOM)
else if (scout_state_.rear_light_state.mode == LIGHT_MODE_CUSTOM)
rear_mode_str += "CUSTOM";
mvwprintw(body_info_win_, linear_axis_negative_y + 6, angular_axis_origin_x - 13, rear_mode_str.c_str());
std::string rear_custom_str = "Custom: " + ConvertFloatToString(scout_state_.rear_light_state.custom_value, 0);
mvwprintw(body_info_win_, linear_axis_negative_y + 6, angular_axis_origin_x + 3, rear_custom_str.c_str());
if (scout_state_.rear_light_state.mode != CONST_OFF &&
!(scout_state_.rear_light_state.mode == CUSTOM && scout_state_.rear_light_state.custom_value == 0))
if (scout_state_.rear_light_state.mode != LIGHT_MODE_CONST_OFF &&
!(scout_state_.rear_light_state.mode == LIGHT_MODE_CUSTOM && scout_state_.rear_light_state.custom_value == 0))
{
NColors::WSetColor(body_info_win_, NColors::BRIGHT_RED);
for (int i = angular_axis_origin_x - 5; i < angular_axis_origin_x - 1; ++i)
@@ -448,19 +449,19 @@ void ScoutMonitor::UpdateScoutSystemInfo()
const int sec1 = static_cast<int>(std::round((si_win_sy_ - 20) / 2.0));
ShowStatusItemName(sec1, state_title_col, "System state");
if (scout_state_.base_state == BASE_NORMAL)
if (scout_state_.base_state == BASE_STATE_NORMAL)
{
NColors::WSetColor(system_info_win_, NColors::GREEN);
mvwprintw(system_info_win_, sec1, state_value_col, "NORMAL");
NColors::WUnsetColor(system_info_win_, NColors::GREEN);
}
else if (scout_state_.base_state == BASE_ESTOP)
else if (scout_state_.base_state == BASE_STATE_ESTOP)
{
NColors::WSetColor(system_info_win_, NColors::YELLOW);
mvwprintw(system_info_win_, sec1, state_value_col, "ESTOP");
NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
}
else if (scout_state_.base_state == BASE_EXCEPTION)
else if (scout_state_.base_state == BASE_STATE_EXCEPTION)
{
NColors::WSetColor(system_info_win_, NColors::RED);
mvwprintw(system_info_win_, sec1, state_value_col, "EXCEPT");
@@ -469,12 +470,12 @@ void ScoutMonitor::UpdateScoutSystemInfo()
// control mode
ShowStatusItemName(sec1 + 1, state_title_col, "Control mode");
if (scout_state_.control_mode == REMOTE_MODE)
if (scout_state_.control_mode == CTRL_MODE_REMOTE)
mvwprintw(system_info_win_, sec1 + 1, state_value_col, "REMOTE");
else if (scout_state_.control_mode == CMD_CAN_MODE)
mvwprintw(system_info_win_, sec1 + 1, state_value_col, "CAN");
else if (scout_state_.control_mode == CMD_UART_MODE)
mvwprintw(system_info_win_, sec1 + 1, state_value_col, "UART");
else if (scout_state_.control_mode == CTRL_MODE_CMD_CAN)
mvwprintw(system_info_win_, sec1 + 1, state_value_col, "CMD");
else if (scout_state_.control_mode == CTRL_MODE_CMD_UART)
mvwprintw(system_info_win_, sec1 + 1, state_value_col, "CMD");
// mvwprintw(system_info_win_, sec1 + 1, state_value_col, std::to_string(scout_state_.control_mode).c_str());
// battery voltage
@@ -491,8 +492,8 @@ void ScoutMonitor::UpdateScoutSystemInfo()
// motor driver over heat;
ShowStatusItemName(sec2 + 1, state_title_col, "-Drv over-heat");
if ((scout_state_.fault_code & MOTOR_DRV_OVERHEAT_W) == 0 &&
(scout_state_.fault_code & MOTOR_DRV_OVERHEAT_F) == 0)
if ((scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_W) == 0 &&
(scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_F) == 0)
{
NColors::WSetColor(system_info_win_, NColors::GREEN);
mvwprintw(system_info_win_, sec2 + 1, fault_col_1, "N");
@@ -500,13 +501,13 @@ void ScoutMonitor::UpdateScoutSystemInfo()
}
else
{
if (scout_state_.fault_code & MOTOR_DRV_OVERHEAT_W)
if (scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_W)
{
NColors::WSetColor(system_info_win_, NColors::YELLOW);
mvwprintw(system_info_win_, sec2 + 1, fault_col_2, "W");
NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
}
if (scout_state_.fault_code & MOTOR_DRV_OVERHEAT_F)
if (scout_state_.fault_code & FAULT_MOTOR_DRV_OVERHEAT_F)
{
NColors::WSetColor(system_info_win_, NColors::RED);
mvwprintw(system_info_win_, sec2 + 1, fault_col_3, "P");
@@ -516,8 +517,8 @@ void ScoutMonitor::UpdateScoutSystemInfo()
// motor driver over current
ShowStatusItemName(sec2 + 2, state_title_col, "-Mt over-current");
if ((scout_state_.fault_code & MOTOR_OVERCURRENT_W) == 0 &&
(scout_state_.fault_code & MOTOR_OVERCURRENT_F) == 0)
if ((scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_W) == 0 &&
(scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_F) == 0)
{
NColors::WSetColor(system_info_win_, NColors::GREEN);
mvwprintw(system_info_win_, sec2 + 2, fault_col_1, "N");
@@ -525,13 +526,13 @@ void ScoutMonitor::UpdateScoutSystemInfo()
}
else
{
if (scout_state_.fault_code & MOTOR_OVERCURRENT_W)
if (scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_W)
{
NColors::WSetColor(system_info_win_, NColors::YELLOW);
mvwprintw(system_info_win_, sec2 + 2, fault_col_2, "W");
NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
}
if (scout_state_.fault_code & MOTOR_OVERCURRENT_F)
if (scout_state_.fault_code & FAULT_MOTOR_OVERCURRENT_F)
{
NColors::WSetColor(system_info_win_, NColors::RED);
mvwprintw(system_info_win_, sec2 + 2, fault_col_3, "P");
@@ -541,8 +542,8 @@ void ScoutMonitor::UpdateScoutSystemInfo()
// battery under voltage
ShowStatusItemName(sec2 + 3, state_title_col, "-Bat under volt");
if ((scout_state_.fault_code & BAT_UNDER_VOL_W) == 0 &&
(scout_state_.fault_code & BAT_UNDER_VOL_F) == 0)
if ((scout_state_.fault_code & FAULT_BAT_UNDER_VOL_W) == 0 &&
(scout_state_.fault_code & FAULT_BAT_UNDER_VOL_F) == 0)
{
NColors::WSetColor(system_info_win_, NColors::GREEN);
mvwprintw(system_info_win_, sec2 + 3, fault_col_1, "N");
@@ -550,13 +551,13 @@ void ScoutMonitor::UpdateScoutSystemInfo()
}
else
{
if (scout_state_.fault_code & BAT_UNDER_VOL_W)
if (scout_state_.fault_code & FAULT_BAT_UNDER_VOL_W)
{
NColors::WSetColor(system_info_win_, NColors::YELLOW);
mvwprintw(system_info_win_, sec2 + 3, fault_col_2, "W");
NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
}
if (scout_state_.fault_code & BAT_UNDER_VOL_F)
if (scout_state_.fault_code & FAULT_BAT_UNDER_VOL_F)
{
NColors::WSetColor(system_info_win_, NColors::RED);
mvwprintw(system_info_win_, sec2 + 3, fault_col_3, "F");
@@ -566,39 +567,39 @@ void ScoutMonitor::UpdateScoutSystemInfo()
// battery over voltage
ShowStatusItemName(sec2 + 4, state_title_col, "-Bat over volt");
ShowFault(sec2 + 4, fault_col_1, (scout_state_.fault_code & BAT_OVER_VOL_F) == 0);
ShowFault(sec2 + 4, fault_col_1, (scout_state_.fault_code & FAULT_BAT_OVER_VOL_F) == 0);
const int sec3 = sec2 + 6;
mvwprintw(system_info_win_, sec3, state_title_col, "Comm faults");
// CAN cmd checksum
ShowStatusItemName(sec3 + 1, state_title_col, "-CAN cmd checksum");
ShowFault(sec3 + 1, fault_col_1, (scout_state_.fault_code & CAN_CHECKSUM_ERROR) == 0);
ShowFault(sec3 + 1, fault_col_1, (scout_state_.fault_code & FAULT_CAN_CHECKSUM_ERROR) == 0);
// motor comm
ShowStatusItemName(sec3 + 2, state_title_col, "-Motor 1 comm");
ShowFault(sec3 + 2, fault_col_1, (scout_state_.fault_code & MOTOR1_COMM_F) == 0);
ShowFault(sec3 + 2, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR1_COMM_F) == 0);
ShowStatusItemName(sec3 + 3, state_title_col, "-Motor 2 comm");
ShowFault(sec3 + 3, fault_col_1, (scout_state_.fault_code & MOTOR2_COMM_F) == 0);
ShowFault(sec3 + 3, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR2_COMM_F) == 0);
ShowStatusItemName(sec3 + 4, state_title_col, "-Motor 3 comm");
ShowFault(sec3 + 4, fault_col_1, (scout_state_.fault_code & MOTOR3_COMM_F) == 0);
ShowFault(sec3 + 4, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR3_COMM_F) == 0);
ShowStatusItemName(sec3 + 5, state_title_col, "-Motor 4 comm");
ShowFault(sec3 + 5, fault_col_1, (scout_state_.fault_code & MOTOR4_COMM_F) == 0);
ShowFault(sec3 + 5, fault_col_1, (scout_state_.fault_code & FAULT_MOTOR4_COMM_F) == 0);
const int sec4 = sec3 + 8;
NColors::WSetColor(system_info_win_, NColors::GREEN);
mvwprintw(system_info_win_, sec4, state_title_col + 1, "N: normal");
mvwprintw(system_info_win_, sec4, state_title_col + 1, "N: Normal");
NColors::WUnsetColor(system_info_win_, NColors::GREEN);
NColors::WSetColor(system_info_win_, NColors::YELLOW);
mvwprintw(system_info_win_, sec4, state_title_col + 12, "W: warning");
mvwprintw(system_info_win_, sec4, state_title_col + 12, "W: Warning");
NColors::WUnsetColor(system_info_win_, NColors::YELLOW);
NColors::WSetColor(system_info_win_, NColors::RED);
mvwprintw(system_info_win_, sec4 + 1, state_title_col + 1, "F: fault P: protection");
mvwprintw(system_info_win_, sec4 + 1, state_title_col + 1, "F: Fault P: Protection");
NColors::WUnsetColor(system_info_win_, NColors::RED);
wrefresh(system_info_win_);

8
scout_base/src/scout_sdk/src/monitor/tests/CMakeLists.txt → scout_base/src/scout_sdk/src/apps/scout_monitor/tests/CMakeLists.txt
View File

@@ -8,8 +8,8 @@ target_link_libraries(test_scout_monitor_virtual monitor)
# add_executable(test_ncurses test_ncurses.cpp)
# target_link_libraries(test_ncurses monitor)
add_executable(test_ncolor test_ncolor.c)
target_link_libraries(test_ncolor monitor)
# add_executable(test_ncolor test_ncolor.c)
# target_link_libraries(test_ncolor monitor)
add_executable(test_ncolor2 test_ncolor2.cpp)
target_link_libraries(test_ncolor2 monitor)
# add_executable(test_ncolor2 test_ncolor2.cpp)
# target_link_libraries(test_ncolor2 monitor)

0
scout_base/src/scout_sdk/src/monitor/tests/test_ncolor.c → scout_base/src/scout_sdk/src/apps/scout_monitor/tests/test_ncolor.c
View File

0
scout_base/src/scout_sdk/src/monitor/tests/test_ncolor2.cpp → scout_base/src/scout_sdk/src/apps/scout_monitor/tests/test_ncolor2.cpp
View File

0
scout_base/src/scout_sdk/src/monitor/tests/test_ncurses.cpp → scout_base/src/scout_sdk/src/apps/scout_monitor/tests/test_ncurses.cpp
View File

0
scout_base/src/scout_sdk/src/monitor/tests/test_scout_monitor.cpp → scout_base/src/scout_sdk/src/apps/scout_monitor/tests/test_scout_monitor.cpp
View File

0
scout_base/src/scout_sdk/src/monitor/tests/test_scout_monitor_virtual.cpp → scout_base/src/scout_sdk/src/apps/scout_monitor/tests/test_scout_monitor_virtual.cpp
View File

3
scout_base/src/scout_sdk/src/comm/CMakeLists.txt Executable file
View File

@@ -0,0 +1,3 @@
# Add source directories
add_subdirectory(async_io)
add_subdirectory(scout_protocol)

22
scout_base/src/scout_sdk/src/comm/async_io/CMakeLists.txt Normal file
View File

@@ -0,0 +1,22 @@
# Dependency libraries
# find_package(LIB_NAME REQUIRED)
# Add libraries
set(ASYNC_IO_LIB_SRC
src/async_serial.cpp
src/async_can.cpp
src/asyncio_utils.cpp
)
add_library(asyncio STATIC ${ASYNC_IO_LIB_SRC})
set_target_properties(asyncio PROPERTIES POSITION_INDEPENDENT_CODE ON)
target_compile_definitions(asyncio PUBLIC "-DASIO_ENABLE_OLD_SERVICES -DASIO_HAS_POSIX_STREAM_DESCRIPTOR")
target_link_libraries(asyncio asio pthread)
target_include_directories(asyncio PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
# Add executables
if(BUILD_TESTS)
add_subdirectory(tests)
endif()

0
scout_base/src/scout_sdk/src/sdk_core/async_io/include/async_io/async_can.hpp → scout_base/src/scout_sdk/src/comm/async_io/include/async_io/async_can.hpp
View File

26
scout_base/src/scout_sdk/src/sdk_core/async_io/include/async_io/async_serial.hpp → scout_base/src/scout_sdk/src/comm/async_io/include/async_io/async_serial.hpp
View File

@@ -18,6 +18,7 @@
* [1] http://www.webalice.it/fede.tft/serial_port/serial_port.html
*
*/
/*
* libmavconn
* Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved.
@@ -47,13 +48,19 @@ namespace wescore
using steady_clock = std::chrono::steady_clock;
using lock_guard = std::lock_guard<std::recursive_mutex>;
/// Note: instance of ASyncSerial MUST be managed by a std::shared_ptr<ASyncSerial>
class ASyncSerial : public std::enable_shared_from_this<ASyncSerial>
{
public:
public:
static constexpr auto DEFAULT_DEVICE = "/dev/ttyUSB0";
static constexpr auto DEFAULT_BAUDRATE = 115200;
static constexpr std::size_t MAX_TXQ_SIZE = 1000;
/**
* @param buf: pointer to a buffer
* @param bufsize: size of the buffer (the value should be constant in most cases)
* @param bytes_received: number of bytes received inside the buffer
*/
using ReceiveCallback = std::function<void(uint8_t *buf, const size_t bufsize, size_t bytes_received)>;
using ClosedCallback = std::function<void(void)>;
@@ -69,7 +76,7 @@ class ASyncSerial : public std::enable_shared_from_this<ASyncSerial>
float rx_speed; //!< current receive speed [B/s]
};
public:
public:
ASyncSerial(std::string device = DEFAULT_DEVICE, unsigned baudrate = DEFAULT_BAUDRATE, bool hwflow = false);
~ASyncSerial();
@@ -80,20 +87,27 @@ class ASyncSerial : public std::enable_shared_from_this<ASyncSerial>
std::size_t conn_id;
static Ptr open_url(std::string url);
void open(std::string device, unsigned baudrate, bool hwflow);
void open(std::string device = "", unsigned baudrate = 0, bool hwflow = false);
void close();
void set_baud(unsigned baudrate) { serial_dev_.set_option(asio::serial_port_base::baud_rate(baudrate)); }
void send_bytes(const uint8_t *bytes, size_t length);
void set_receive_callback(ReceiveCallback cb) { receive_cb = cb; }
void set_closed_callback(ClosedCallback cb) { port_closed_cb = cb; }
inline bool is_open() { return serial_dev.is_open(); }
inline bool is_open() { return serial_dev_.is_open(); }
IOStat get_iostat();
private:
private:
// monotonic counter (increment only)
static std::atomic<std::size_t> conn_id_counter;
// port properties
std::string device_ = DEFAULT_DEVICE;
unsigned baudrate_ = DEFAULT_BAUDRATE;
bool hwflow_ = false;
// port statistics
std::atomic<std::size_t> tx_total_bytes;
std::atomic<std::size_t> rx_total_bytes;
@@ -105,7 +119,7 @@ class ASyncSerial : public std::enable_shared_from_this<ASyncSerial>
// io service
asio::io_service io_service;
std::thread io_thread;
asio::serial_port serial_dev;
asio::serial_port serial_dev_;
std::atomic<bool> tx_in_progress;
std::deque<MsgBuffer> tx_q;

0
scout_base/src/scout_sdk/src/sdk_core/async_io/include/async_io/device_error.hpp → scout_base/src/scout_sdk/src/comm/async_io/include/async_io/device_error.hpp
View File

0
scout_base/src/scout_sdk/src/sdk_core/async_io/include/async_io/msg_buffer.hpp → scout_base/src/scout_sdk/src/comm/async_io/include/async_io/msg_buffer.hpp
View File

0
scout_base/src/scout_sdk/src/sdk_core/async_io/src/async_can.cpp → scout_base/src/scout_sdk/src/comm/async_io/src/async_can.cpp
View File

48
scout_base/src/scout_sdk/src/sdk_core/async_io/src/async_serial.cpp → scout_base/src/scout_sdk/src/comm/async_io/src/async_serial.cpp
View File

@@ -41,7 +41,9 @@ using std::error_code;
std::atomic<size_t> ASyncSerial::conn_id_counter{0};
ASyncSerial::ASyncSerial(std::string device, unsigned baudrate, bool hwflow) : tx_total_bytes(0),
ASyncSerial::ASyncSerial(std::string device, unsigned baudrate, bool hwflow) : device_(device),
baudrate_(baudrate),
hwflow_(hwflow), tx_total_bytes(0),
rx_total_bytes(0),
last_tx_total_bytes(0),
last_rx_total_bytes(0),
@@ -50,10 +52,9 @@ ASyncSerial::ASyncSerial(std::string device, unsigned baudrate, bool hwflow) : t
tx_q{},
rx_buf{},
io_service(),
serial_dev(io_service)
serial_dev_(io_service)
{
conn_id = conn_id_counter.fetch_add(1);
open(device, baudrate, hwflow);
}
ASyncSerial::~ASyncSerial()
@@ -65,23 +66,30 @@ void ASyncSerial::open(std::string device, unsigned baudrate, bool hwflow)
{
using SPB = asio::serial_port_base;
std::cout << "connection: " << conn_id << " , device: " << device.c_str() << " @ " << baudrate << "bps" << std::endl;
if (device != "")
{
device_ = device;
baudrate_ = baudrate;
hwflow_ = hwflow;
}
std::cout << "connection: " << conn_id << " , device: " << device_ << " @ " << baudrate_ << "bps" << std::endl;
try
{
serial_dev.open(device);
serial_dev_.open(device_);
// Set baudrate and 8N1 mode
serial_dev.set_option(SPB::baud_rate(baudrate));
serial_dev.set_option(SPB::character_size(8));
serial_dev.set_option(SPB::parity(SPB::parity::none));
serial_dev.set_option(SPB::stop_bits(SPB::stop_bits::one));
serial_dev.set_option(SPB::flow_control((hwflow) ? SPB::flow_control::hardware : SPB::flow_control::none));
serial_dev_.set_option(SPB::baud_rate(baudrate_));
serial_dev_.set_option(SPB::character_size(8));
serial_dev_.set_option(SPB::parity(SPB::parity::none));
serial_dev_.set_option(SPB::stop_bits(SPB::stop_bits::one));
serial_dev_.set_option(SPB::flow_control((hwflow_) ? SPB::flow_control::hardware : SPB::flow_control::none));
#if defined(__linux__)
// Enable low latency mode on Linux
{
int fd = serial_dev.native_handle();
int fd = serial_dev_.native_handle();
struct serial_struct ser_info;
ioctl(fd, TIOCGSERIAL, &ser_info);
@@ -99,6 +107,7 @@ void ASyncSerial::open(std::string device, unsigned baudrate, bool hwflow)
// NOTE: shared_from_this() should not be used in constructors
// TODO is the following step necessary?
// give some work to io_service before start
io_service.post(std::bind(&ASyncSerial::do_read, this));
@@ -115,8 +124,8 @@ void ASyncSerial::close()
if (!is_open())
return;
serial_dev.cancel();
serial_dev.close();
serial_dev_.cancel();
serial_dev_.close();
io_service.stop();
@@ -168,19 +177,14 @@ void ASyncSerial::send_bytes(const uint8_t *bytes, size_t length)
{
if (!is_open())
{
std::cerr << "send: channel closed!"
<< " connection id: " << conn_id << std::endl;
std::cerr << "send: channel closed! connection id: " << conn_id << std::endl;
return;
}
{
lock_guard lock(mutex);
if (tx_q.size() >= MAX_TXQ_SIZE)
throw std::length_error("ASyncSerial::send_bytes: TX queue overflow");
tx_q.emplace_back(bytes, length);
}
io_service.post(std::bind(&ASyncSerial::do_write, shared_from_this(), true));
}
@@ -207,7 +211,7 @@ void ASyncSerial::default_receive_callback(uint8_t *buf, const size_t bufsize, s
void ASyncSerial::do_read(void)
{
auto sthis = shared_from_this();
serial_dev.async_read_some(
serial_dev_.async_read_some(
buffer(rx_buf),
[sthis](error_code error, size_t bytes_transferred) {
if (error)
@@ -237,7 +241,7 @@ void ASyncSerial::do_write(bool check_tx_state)
tx_in_progress = true;
auto sthis = shared_from_this();
auto &buf_ref = tx_q.front();
serial_dev.async_write_some(
serial_dev_.async_write_some(
buffer(buf_ref.dpos(), buf_ref.nbytes()),
[sthis, &buf_ref](error_code error, size_t bytes_transferred) {
assert(bytes_transferred <= buf_ref.len);
@@ -261,9 +265,7 @@ void ASyncSerial::do_write(bool check_tx_state)
buf_ref.pos += bytes_transferred;
if (buf_ref.nbytes() == 0)
{
sthis->tx_q.pop_front();
}
if (!sthis->tx_q.empty())
sthis->do_write(false);

82
scout_base/src/scout_sdk/src/comm/async_io/src/asyncio_utils.cpp Normal file
View File

@@ -0,0 +1,82 @@
/*
* asyncio_utils.cpp
*
* Created on: Jul 24, 2019 01:46
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "asyncio_utils.hpp"
namespace wescore
{
void url_parse_host(std::string host,
std::string &host_out, int &port_out,
const std::string def_host, const int def_port)
{
std::string port;
auto sep_it = std::find(host.begin(), host.end(), ':');
if (sep_it == host.end())
{
// host
if (!host.empty())
{
host_out = host;
port_out = def_port;
}
else
{
host_out = def_host;
port_out = def_port;
}
return;
}
if (sep_it == host.begin())
{
// :port
host_out = def_host;
}
else
{
// host:port
host_out.assign(host.begin(), sep_it);
}
port.assign(sep_it + 1, host.end());
port_out = std::stoi(port);
}
/**
* Parse ?ids=sid,cid
*/
void url_parse_query(std::string query)
{
const std::string ids_end("ids=");
std::string sys, comp;
if (query.empty())
return;
auto ids_it = std::search(query.begin(), query.end(),
ids_end.begin(), ids_end.end());
if (ids_it == query.end())
{
std::cerr << "URL: unknown query arguments" << std::endl;
return;
}
std::advance(ids_it, ids_end.length());
auto comma_it = std::find(ids_it, query.end(), ',');
if (comma_it == query.end())
{
std::cerr << "URL: no comma in ids= query" << std::endl;
return;
}
sys.assign(ids_it, comma_it);
comp.assign(comma_it + 1, query.end());
}
} // namespace wescore

64
scout_base/src/scout_sdk/src/sdk_core/async_io/src/asyncio_utils.hpp → scout_base/src/scout_sdk/src/comm/async_io/src/asyncio_utils.hpp
View File

@@ -52,72 +52,12 @@ bool set_this_thread_name(const std::string &name, Args &&... args)
*/
void url_parse_host(std::string host,
std::string &host_out, int &port_out,
const std::string def_host, const int def_port)
{
std::string port;
auto sep_it = std::find(host.begin(), host.end(), ':');
if (sep_it == host.end())
{
// host
if (!host.empty())
{
host_out = host;
port_out = def_port;
}
else
{
host_out = def_host;
port_out = def_port;
}
return;
}
if (sep_it == host.begin())
{
// :port
host_out = def_host;
}
else
{
// host:port
host_out.assign(host.begin(), sep_it);
}
port.assign(sep_it + 1, host.end());
port_out = std::stoi(port);
}
const std::string def_host, const int def_port);
/**
* Parse ?ids=sid,cid
*/
void url_parse_query(std::string query)
{
const std::string ids_end("ids=");
std::string sys, comp;
if (query.empty())
return;
auto ids_it = std::search(query.begin(), query.end(),
ids_end.begin(), ids_end.end());
if (ids_it == query.end())
{
std::cerr << "URL: unknown query arguments" << std::endl;
return;
}
std::advance(ids_it, ids_end.length());
auto comma_it = std::find(ids_it, query.end(), ',');
if (comma_it == query.end())
{
std::cerr << "URL: no comma in ids= query" << std::endl;
return;
}
sys.assign(ids_it, comma_it);
comp.assign(comma_it + 1, query.end());
}
void url_parse_query(std::string query);
} // namespace wescore
#endif /* ASYNCIO_UTILS_HPP */

3
scout_base/src/scout_sdk/src/sdk_core/async_io/tests/CMakeLists.txt → scout_base/src/scout_sdk/src/comm/async_io/tests/CMakeLists.txt
View File

@@ -2,6 +2,9 @@
add_executable(test_aserial test_aserial.cpp)
target_link_libraries(test_aserial asyncio)
add_executable(test_aserial_comm test_aserial_comm.cpp)
target_link_libraries(test_aserial_comm asyncio)
add_executable(test_asio_can test_asio_can.cpp)
target_link_libraries(test_asio_can asio pthread)

0
scout_base/src/scout_sdk/src/sdk_core/async_io/tests/test_acan.cpp → scout_base/src/scout_sdk/src/comm/async_io/tests/test_acan.cpp
View File

42
scout_base/src/scout_sdk/src/comm/async_io/tests/test_aserial.cpp Normal file
View File

@@ -0,0 +1,42 @@
/*
* test_interface.cpp
*
* Created on: Dec 25, 2016
* Author: rdu
*/
#include <iostream>
#include "async_io/async_serial.hpp"
using namespace wescore;
void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
{
std::cout << "parser called" << std::endl;
for (; bytes_received > 0; bytes_received--)
{
auto c = *buf++;
}
}
int main(int argc, char *argv[])
{
// ASyncSerial::Ptr serial = ASyncSerial::open_url("/dev/ttyUSB0:115200");
std::shared_ptr<ASyncSerial> serial = std::make_shared<ASyncSerial>("/dev/ttyO5", 115200);
serial->set_receive_callback(parse_buffer);
if (serial->is_open())
std::cout << "serial port opened" << std::endl;
uint8_t data[8] = {'a','b','c'};
while (1)
{
// serial->send_bytes(data, 3);
sleep(1);
}
}

82
scout_base/src/scout_sdk/src/comm/async_io/tests/test_aserial_comm.cpp Normal file
View File

@@ -0,0 +1,82 @@
/*
* test_interface.cpp
*
* Created on: Dec 25, 2016
* Author: rdu
*/
#include <iostream>
#include "async_io/async_serial.hpp"
using namespace wescore;
void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
{
// std::cout << "parser called" << std::endl;
// for (int i = 0; i < bytes_received; ++i)
// {
// // auto c = *buf++;
// std::cout << std::hex << static_cast<int>(buf[i]) << std::dec << " ";
// }
if (bytes_received > 2)
{
for (int i = 0; i < bytes_received - 1; ++i)
{
uint8_t first = buf[i];
uint8_t second = buf[i + 1];
if (first == 0xB5 && second == 0x62)
std::cout << "- start bytes found" << std::endl;
// std::cout << std::hex << static_cast<int>(buf[i]) << std::dec << " ";
}
}
}
int main(int argc, char *argv[])
{
std::string device_name;
int baud;
if (argc == 3)
{
device_name = {argv[1]};
baud = std::stoi(argv[2]);
std::cout << "Specified device: " << device_name << ", baud: " << baud << std::endl;
}
else
{
std::cout << "Usage: test_aserial_comm <interface> <baud>" << std::endl
<< "Example: ./test_aserial_comm /dev/ttyUSB0 115200" << std::endl;
return -1;
}
std::shared_ptr<ASyncSerial> serial = std::make_shared<ASyncSerial>(device_name, baud);
serial->set_receive_callback(parse_buffer);
if (serial->is_open())
std::cout << "serial port opened" << std::endl;
uint8_t data[8] = {'a', 'b', 'c'};
int count = 0;
uint8_t idx = 0;
const unsigned baudrates[] = {9600, 57600, 115200};
while (1)
{
// serial->send_bytes(data, 3);
if (++count == 15)
{
count = 0;
std::cout << "----------------------------------------" << std::endl;
std::cout << "change baud rate to " << baudrates[idx % 3] << std::endl;
std::cout << "----------------------------------------" << std::endl;
serial->set_baud(baudrates[idx % 3]);
idx++;
}
sleep(1);
}
}

0
scout_base/src/scout_sdk/src/sdk_core/async_io/tests/test_asio_can.cpp → scout_base/src/scout_sdk/src/comm/async_io/tests/test_asio_can.cpp
View File

12
scout_base/src/scout_sdk/src/comm/scout_protocol/CMakeLists.txt Executable file
View File

@@ -0,0 +1,12 @@
## Add libraries
set(SCOUT_PROTOCOL_SRC
src/scout_can_parser.c
src/scout_uart_parser.c
)
add_library(scout_protocol STATIC ${SCOUT_PROTOCOL_SRC})
set_target_properties(scout_protocol PROPERTIES POSITION_INDEPENDENT_CODE ON)
target_include_directories(scout_protocol PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<BUILD_INTERFACE:${Boost_INCLUDE_DIRS}>
$<INSTALL_INTERFACE:include>
PRIVATE src)

3
scout_base/src/scout_sdk/src/comm/scout_protocol/README.md Normal file
View File

@@ -0,0 +1,3 @@
## AgileX Protocol
This module is implemented in C, designed for exchanging information with AgileX mobile bases.

48
scout_base/src/scout_sdk/src/comm/scout_protocol/include/scout_protocol/scout_can_parser.h Normal file
View File

@@ -0,0 +1,48 @@
/*
* 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 <stdint.h>
#include <stdbool.h>
#include "scout_protocol/scout_protocol.h"
#ifdef __linux__
#include <linux/can.h>
#else
struct can_frame
{
uint32_t can_id;
uint8_t can_dlc;
uint8_t data[8]__attribute__((aligned(8)));
};
#endif
bool DecodeScoutStatusMsgFromCAN(const struct can_frame *rx_frame, ScoutStatusMessage *msg);
bool DecodeScoutControlMsgFromCAN(const struct can_frame *rx_frame, ScoutControlMessage *msg);
void EncodeScoutStatusMsgToCAN(const ScoutStatusMessage *msg, struct can_frame *tx_frame);
void EncodeScoutControlMsgToCAN(const ScoutControlMessage *msg, struct can_frame *tx_frame);
void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame);
void EncodeScoutLightControlMsgToCAN(const LightControlMessage *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 */

283
scout_base/src/scout_sdk/src/comm/scout_protocol/include/scout_protocol/scout_protocol.h Normal file
View File

@@ -0,0 +1,283 @@
/*
* 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 <stdint.h>
#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_HIGH_BYTE_RESERVED1 ((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
{
ScoutStatusNone = 0x00,
ScoutMotionStatusMsg = 0x01,
ScoutLightStatusMsg = 0x02,
ScoutSystemStatusMsg = 0x03,
ScoutMotorDriverStatusMsg = 0x04
} ScoutStatusMsgType;
typedef struct
{
ScoutStatusMsgType msg_type;
// only one of the following fields is updated, as specified by msg_type
MotionStatusMessage motion_status_msg;
LightStatusMessage light_status_msg;
SystemStatusMessage system_status_msg;
MotorDriverStatusMessage motor_driver_status_msg;
} ScoutStatusMessage;
typedef enum
{
ScoutControlNone = 0x20,
ScoutMotionControlMsg = 0x21,
ScoutLightControlMsg = 0x22
} ScoutControlMsgType;
typedef struct
{
ScoutControlMsgType msg_type;
// only one of the following fields is updated, as specified by msg_type
MotionControlMessage motion_control_msg;
LightControlMessage light_control_msg;
} ScoutControlMessage;
#pragma pack(pop)
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_PROTOCOL_H */

38
scout_base/src/scout_sdk/src/comm/scout_protocol/include/scout_protocol/scout_uart_parser.h Normal file
View File

@@ -0,0 +1,38 @@
/*
* scout_uart_parser.h
*
* Created on: Aug 14, 2019 12:01
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_UART_PARSER_H
#define SCOUT_UART_PARSER_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <string.h>
#include <stdbool.h>
#include "scout_protocol/scout_protocol.h"
bool DecodeScoutStatusMsgFromUART(uint8_t c, ScoutStatusMessage *msg);
bool DecodeScoutControlMsgFromUART(uint8_t c, ScoutControlMessage *msg);
void EncodeScoutStatusMsgToUART(const ScoutStatusMessage *msg, uint8_t *buf, uint8_t *len);
void EncodeScoutControlMsgToUART(const ScoutControlMessage *msg, uint8_t *buf, uint8_t *len);
void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len);
void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len);
uint8_t CalcScoutUARTChecksum(uint8_t *buf, uint8_t len);
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_UART_PARSER_H */

197
scout_base/src/scout_sdk/src/comm/scout_protocol/src/scout_can_parser.c Normal file
View File

@@ -0,0 +1,197 @@
/*
* scout_can_parser.c
*
* Created on: Aug 31, 2019 04:25
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "scout_protocol/scout_can_parser.h"
#include "string.h"
bool DecodeScoutStatusMsgFromCAN(const struct can_frame *rx_frame, ScoutStatusMessage *msg)
{
msg->msg_type = ScoutStatusNone;
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->msg_type = ScoutMotionStatusMsg;
// msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
memcpy(msg->motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_LIGHT_CONTROL_STATUS_ID:
{
msg->msg_type = ScoutLightStatusMsg;
// msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
memcpy(msg->light_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_SYSTEM_STATUS_STATUS_ID:
{
msg->msg_type = ScoutSystemStatusMsg;
// msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
memcpy(msg->system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR1_DRIVER_STATUS_ID:
{
msg->msg_type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
msg->motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID;
memcpy(msg->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->msg_type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
msg->motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID;
memcpy(msg->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->msg_type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
msg->motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID;
memcpy(msg->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->msg_type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
msg->motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID;
memcpy(msg->motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
default:
break;
}
return true;
}
bool DecodeScoutControlMsgFromCAN(const struct can_frame *rx_frame, ScoutControlMessage *msg)
{
msg->msg_type = ScoutControlNone;
switch (rx_frame->can_id)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_CONTROL_CMD_ID:
{
msg->msg_type = ScoutMotionControlMsg;
// msg->motion_control_msg.id = CAN_MSG_MOTION_CONTROL_CMD_ID;
memcpy(msg->motion_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_LIGHT_CONTROL_CMD_ID:
{
msg->msg_type = ScoutLightControlMsg;
// msg->light_control_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
memcpy(msg->light_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
default:
break;
}
return true;
}
void EncodeScoutStatusMsgToCAN(const ScoutStatusMessage *msg, struct can_frame *tx_frame)
{
switch (msg->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->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->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->system_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutMotorDriverStatusMsg:
{
if (msg->motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID)
tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
else if (msg->motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID)
tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
else if (msg->motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID)
tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
else if (msg->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->motor_driver_status_msg.data.raw, tx_frame->can_dlc);
break;
}
default:
break;
}
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeScoutControlMsgToCAN(const ScoutControlMessage *msg, struct can_frame *tx_frame)
{
switch (msg->msg_type)
{
case ScoutMotionControlMsg:
{
EncodeScoutMotionControlMsgToCAN(&(msg->motion_control_msg), tx_frame);
break;
}
case ScoutLightControlMsg:
{
EncodeScoutLightControlMsgToCAN(&(msg->light_control_msg), tx_frame);
break;
}
default:
break;
}
}
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;
}

640
scout_base/src/scout_sdk/src/comm/scout_protocol/src/scout_uart_parser.c Normal file
View File

@@ -0,0 +1,640 @@
/*
* scout_uart_parser.c
*
* Created on: Aug 14, 2019 12:02
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "scout_protocol/scout_uart_parser.h"
// #define PRINT_CPP_DEBUG_INFO
// #define PRINT_JLINK_DEBUG_INFO
// #define USE_XOR_CHECKSUM
#ifdef PRINT_CPP_DEBUG_INFO
#undef PRINT_JLINK_DEBUG_INFO
#endif
#ifdef PRINT_CPP_DEBUG_INFO
#define < iostream >
#elif (defined(PRINT_JLINK_DEBUG_INFO))
#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
} ScoutSerialDecodeState;
#define PAYLOAD_BUFFER_SIZE (SCOUT_FRAME_SIZE * 2)
#define FRAME_SOF_LEN ((uint8_t)2)
#define FRAME_FIXED_FIELD_LEN ((uint8_t)4)
#define FRAME_SOF1 ((uint8_t)0x5a)
#define FRAME_SOF2 ((uint8_t)0xa5)
#define FRAME_TYPE_CONTROL ((uint8_t)0x55)
#define FRAME_TYPE_STATUS ((uint8_t)0xaa)
#define FRAME_NONE_ID ((uint8_t)0x00)
typedef union {
ScoutStatusMessage status_msg;
ScoutControlMessage control_msg;
} ScoutDecodedMessage;
// frame buffer
static uint8_t frame_id = 0;
static uint8_t frame_type = 0;
static uint8_t frame_len = 0;
static uint8_t frame_cnt = 0;
static uint8_t frame_checksum = 0;
static uint8_t internal_checksum = 0;
static uint8_t payload_buffer[PAYLOAD_BUFFER_SIZE];
static size_t payload_data_pos = 0;
// statisctics
static uint32_t frame_parsed = 0;
static uint32_t frame_with_wrong_checksum = 0;
static bool ParseChar(uint8_t c, ScoutDecodedMessage *msg);
static uint8_t CalcBufferedFrameChecksum();
static bool ConstructStatusMessage(ScoutStatusMessage *msg);
static bool ConstructControlMessage(ScoutControlMessage *msg);
void EncodeScoutStatusMsgToUART(const ScoutStatusMessage *msg, uint8_t *buf, uint8_t *len)
{
// SOF
buf[0] = FRAME_SOF1;
buf[1] = FRAME_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = FRAME_TYPE_STATUS;
switch (msg->msg_type)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case ScoutMotionStatusMsg:
{
buf[4] = UART_FRAME_MOTION_STATUS_ID;
buf[5] = msg->motion_status_msg.data.status.linear_velocity.high_byte;
buf[6] = msg->motion_status_msg.data.status.linear_velocity.low_byte;
buf[7] = msg->motion_status_msg.data.status.angular_velocity.high_byte;
buf[8] = msg->motion_status_msg.data.status.angular_velocity.low_byte;
buf[9] = 0;
buf[10] = 0;
buf[11] = msg->motion_status_msg.data.status.count;
break;
}
case ScoutLightStatusMsg:
{
buf[4] = UART_FRAME_LIGHT_STATUS_ID;
buf[5] = msg->light_status_msg.data.status.light_ctrl_enable;
buf[6] = msg->light_status_msg.data.status.front_light_mode;
buf[7] = msg->light_status_msg.data.status.front_light_custom;
buf[8] = msg->light_status_msg.data.status.rear_light_mode;
buf[9] = msg->light_status_msg.data.status.rear_light_custom;
buf[10] = 0;
buf[11] = msg->light_status_msg.data.status.count;
break;
}
case ScoutSystemStatusMsg:
{
buf[4] = UART_FRAME_SYSTEM_STATUS_ID;
buf[5] = msg->system_status_msg.data.status.base_state;
buf[6] = msg->system_status_msg.data.status.control_mode;
buf[7] = msg->system_status_msg.data.status.battery_voltage.high_byte;
buf[8] = msg->system_status_msg.data.status.battery_voltage.low_byte;
buf[9] = msg->system_status_msg.data.status.fault_code.high_byte;
buf[10] = msg->system_status_msg.data.status.fault_code.low_byte;
buf[11] = msg->system_status_msg.data.status.count;
break;
}
case ScoutMotorDriverStatusMsg:
{
if (msg->motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID)
buf[4] = UART_FRAME_MOTOR1_DRIVER_STATUS_ID;
else if (msg->motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID)
buf[4] = UART_FRAME_MOTOR2_DRIVER_STATUS_ID;
else if (msg->motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID)
buf[4] = UART_FRAME_MOTOR3_DRIVER_STATUS_ID;
else if (msg->motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID)
buf[4] = UART_FRAME_MOTOR4_DRIVER_STATUS_ID;
buf[5] = msg->motor_driver_status_msg.data.status.current.high_byte;
buf[6] = msg->motor_driver_status_msg.data.status.current.low_byte;
buf[7] = msg->motor_driver_status_msg.data.status.rpm.high_byte;
buf[8] = msg->motor_driver_status_msg.data.status.rpm.low_byte;
buf[9] = msg->motor_driver_status_msg.data.status.temperature;
buf[10] = 0;
buf[11] = msg->motor_driver_status_msg.data.status.count;
break;
}
default:
break;
}
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + FRAME_SOF_LEN + 1;
}
void EncodeScoutControlMsgToUART(const ScoutControlMessage *msg, uint8_t *buf, uint8_t *len)
{
switch (msg->msg_type)
{
case ScoutMotionControlMsg:
{
EncodeMotionControlMsgToUART(&(msg->motion_control_msg), buf, len);
break;
}
case ScoutLightControlMsg:
{
EncodeLightControlMsgToUART(&(msg->light_control_msg), buf, len);
break;
}
default:
break;
}
}
void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len)
{
// SOF
buf[0] = FRAME_SOF1;
buf[1] = FRAME_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = FRAME_TYPE_CONTROL;
buf[4] = UART_FRAME_MOTION_CONTROL_ID;
// frame payload
buf[5] = msg->data.cmd.control_mode;
buf[6] = msg->data.cmd.fault_clear_flag;
buf[7] = msg->data.cmd.linear_velocity_cmd;
buf[8] = msg->data.cmd.angular_velocity_cmd;
buf[9] = 0x00;
buf[10] = 0x00;
// frame count, checksum
buf[11] = msg->data.cmd.count;
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + FRAME_SOF_LEN + 1;
}
void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len)
{
// SOF
buf[0] = FRAME_SOF1;
buf[1] = FRAME_SOF2;
// frame len, type, ID
buf[2] = 0x0a;
buf[3] = FRAME_TYPE_CONTROL;
buf[4] = UART_FRAME_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] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + FRAME_SOF_LEN + 1;
}
bool DecodeScoutStatusMsgFromUART(uint8_t c, ScoutStatusMessage *msg)
{
static ScoutDecodedMessage decoded_msg;
bool result = ParseChar(c, &decoded_msg);
if (result)
*msg = decoded_msg.status_msg;
return result;
}
bool DecodeScoutControlMsgFromUART(uint8_t c, ScoutControlMessage *msg)
{
static ScoutDecodedMessage decoded_msg;
bool result = ParseChar(c, &decoded_msg);
if (result)
*msg = decoded_msg.control_msg;
return result;
}
uint8_t CalcScoutUARTChecksum(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 ^= FRAME_SOF1;
checksum ^= FRAME_SOF2;
checksum ^= frame_len;
checksum ^= frame_type;
checksum ^= frame_id;
for (size_t i = 0; i < payload_data_pos; ++i)
checksum ^= payload_buffer[i];
checksum ^= frame_cnt;
#else
checksum += FRAME_SOF1;
checksum += FRAME_SOF2;
checksum += frame_len;
checksum += frame_type;
checksum += frame_id;
for (size_t i = 0; i < payload_data_pos; ++i)
checksum += payload_buffer[i];
checksum += frame_cnt;
#endif
return checksum;
}
bool ParseChar(uint8_t c, ScoutDecodedMessage *msg)
{
static ScoutSerialDecodeState decode_state = WAIT_FOR_SOF1;
bool new_frame_parsed = false;
switch (decode_state)
{
case WAIT_FOR_SOF1:
{
if (c == FRAME_SOF1)
{
frame_id = FRAME_NONE_ID;
frame_type = 0;
frame_len = 0;
frame_cnt = 0;
frame_checksum = 0;
internal_checksum = 0;
payload_data_pos = 0;
memset(payload_buffer, 0, PAYLOAD_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 == FRAME_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:
{
frame_len = c;
decode_state = WAIT_FOR_FRAME_TYPE;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "frame len: " << std::hex << static_cast<int>(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 FRAME_TYPE_CONTROL:
{
frame_type = FRAME_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 FRAME_TYPE_STATUS:
{
frame_type = FRAME_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 FRAME_TYPE_STATUS" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "ERROR: Not expecting frame of a type other than FRAME_TYPE_STATUS\n");
#endif
decode_state = WAIT_FOR_SOF1;
}
}
break;
}
case WAIT_FOR_FRAME_ID:
{
switch (c)
{
case UART_FRAME_SYSTEM_STATUS_ID:
case UART_FRAME_MOTION_STATUS_ID:
case UART_FRAME_MOTOR1_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR2_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR3_DRIVER_STATUS_ID:
case UART_FRAME_MOTOR4_DRIVER_STATUS_ID:
case UART_FRAME_LIGHT_STATUS_ID:
{
frame_id = c;
decode_state = WAIT_FOR_PAYLOAD;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "frame id: " << std::hex << static_cast<int>(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:
{
payload_buffer[payload_data_pos++] = c;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "1 byte added: " << std::hex << static_cast<int>(c) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkRTTPrintf(0, "1 byte added: %d\n", c);
#endif
if (payload_data_pos == (frame_len - FRAME_FIXED_FIELD_LEN))
decode_state = WAIT_FOR_FRAME_COUNT;
break;
}
case WAIT_FOR_FRAME_COUNT:
{
frame_cnt = c;
decode_state = WAIT_FOR_CHECKSUM;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "frame count: " << std::hex << static_cast<int>(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:
{
frame_checksum = c;
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<int>(frame_checksum) << std::dec << std::endl;
std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(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 (frame_checksum == 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 (frame_type == FRAME_TYPE_STATUS)
ConstructStatusMessage(&(msg->status_msg));
else if (frame_type == FRAME_TYPE_CONTROL)
ConstructControlMessage(&(msg->control_msg));
++frame_parsed;
}
else
{
++frame_with_wrong_checksum;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "checksum is NOT correct" << std::endl;
std::cout << std::hex << static_cast<int>(frame_id) << " , " << static_cast<int>(frame_len) << " , " << static_cast<int>(frame_cnt) << " , " << static_cast<int>(frame_checksum) << " : " << std::dec << std::endl;
std::cout << "payload: ";
for (int i = 0; i < payload_data_pos; ++i)
std::cout << std::hex << static_cast<int>(payload_buffer[i]) << std::dec << " ";
std::cout << std::endl;
std::cout << "--- frame checksum: " << std::hex << static_cast<int>(frame_checksum) << std::dec << std::endl;
std::cout << "--- internal frame checksum: " << std::hex << static_cast<int>(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(ScoutControlMessage *msg)
{
if (msg == NULL)
return false;
switch (frame_id)
{
case UART_FRAME_MOTION_CONTROL_ID:
{
msg->msg_type = ScoutMotionControlMsg;
msg->motion_control_msg.data.cmd.control_mode = payload_buffer[0];
msg->motion_control_msg.data.cmd.fault_clear_flag = payload_buffer[1];
msg->motion_control_msg.data.cmd.linear_velocity_cmd = payload_buffer[2];
msg->motion_control_msg.data.cmd.angular_velocity_cmd = payload_buffer[3];
msg->motion_control_msg.data.cmd.reserved0 = payload_buffer[4];
msg->motion_control_msg.data.cmd.reserved1 = payload_buffer[5];
msg->motion_control_msg.data.cmd.count = frame_cnt;
msg->motion_control_msg.data.cmd.checksum = frame_checksum;
break;
}
case UART_FRAME_LIGHT_CONTROL_ID:
{
msg->msg_type = ScoutLightControlMsg;
msg->light_control_msg.data.cmd.light_ctrl_enable = payload_buffer[0];
msg->light_control_msg.data.cmd.front_light_mode = payload_buffer[1];
msg->light_control_msg.data.cmd.front_light_custom = payload_buffer[2];
msg->light_control_msg.data.cmd.rear_light_mode = payload_buffer[3];
msg->light_control_msg.data.cmd.rear_light_custom = payload_buffer[4];
msg->light_control_msg.data.cmd.reserved0 = payload_buffer[5];
msg->light_control_msg.data.cmd.count = frame_cnt;
msg->light_control_msg.data.cmd.checksum = frame_checksum;
break;
}
}
return true;
}
bool ConstructStatusMessage(ScoutStatusMessage *msg)
{
if (msg == NULL)
return false;
switch (frame_id)
{
case UART_FRAME_SYSTEM_STATUS_ID:
{
msg->msg_type = ScoutSystemStatusMsg;
msg->system_status_msg.data.status.base_state = payload_buffer[0];
msg->system_status_msg.data.status.control_mode = payload_buffer[1];
msg->system_status_msg.data.status.battery_voltage.high_byte = payload_buffer[2];
msg->system_status_msg.data.status.battery_voltage.low_byte = payload_buffer[3];
msg->system_status_msg.data.status.fault_code.high_byte = payload_buffer[4];
msg->system_status_msg.data.status.fault_code.low_byte = payload_buffer[5];
msg->system_status_msg.data.status.count = frame_cnt;
msg->system_status_msg.data.status.checksum = frame_checksum;
break;
}
case UART_FRAME_MOTION_STATUS_ID:
{
msg->msg_type = ScoutMotionStatusMsg;
msg->motion_status_msg.data.status.linear_velocity.high_byte = payload_buffer[0];
msg->motion_status_msg.data.status.linear_velocity.low_byte = payload_buffer[1];
msg->motion_status_msg.data.status.angular_velocity.high_byte = payload_buffer[2];
msg->motion_status_msg.data.status.angular_velocity.low_byte = payload_buffer[3];
msg->motion_status_msg.data.status.reserved0 = 0x00;
msg->motion_status_msg.data.status.reserved0 = 0x00;
msg->motion_status_msg.data.status.count = frame_cnt;
msg->motion_status_msg.data.status.checksum = frame_checksum;
break;
}
case UART_FRAME_MOTOR1_DRIVER_STATUS_ID:
{
msg->msg_type = ScoutMotorDriverStatusMsg;
msg->motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID;
msg->motor_driver_status_msg.data.status.current.high_byte = payload_buffer[0];
msg->motor_driver_status_msg.data.status.current.low_byte = payload_buffer[1];
msg->motor_driver_status_msg.data.status.rpm.high_byte = payload_buffer[2];
msg->motor_driver_status_msg.data.status.rpm.low_byte = payload_buffer[3];
msg->motor_driver_status_msg.data.status.temperature = payload_buffer[4];
msg->motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->motor_driver_status_msg.data.status.count = frame_cnt;
msg->motor_driver_status_msg.data.status.checksum = frame_checksum;
break;
}
case UART_FRAME_MOTOR2_DRIVER_STATUS_ID:
{
msg->msg_type = ScoutMotorDriverStatusMsg;
msg->motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID;
msg->motor_driver_status_msg.data.status.current.high_byte = payload_buffer[0];
msg->motor_driver_status_msg.data.status.current.low_byte = payload_buffer[1];
msg->motor_driver_status_msg.data.status.rpm.high_byte = payload_buffer[2];
msg->motor_driver_status_msg.data.status.rpm.low_byte = payload_buffer[3];
msg->motor_driver_status_msg.data.status.temperature = payload_buffer[4];
msg->motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->motor_driver_status_msg.data.status.count = frame_cnt;
msg->motor_driver_status_msg.data.status.checksum = frame_checksum;
break;
}
case UART_FRAME_MOTOR3_DRIVER_STATUS_ID:
{
msg->msg_type = ScoutMotorDriverStatusMsg;
msg->motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID;
msg->motor_driver_status_msg.data.status.current.high_byte = payload_buffer[0];
msg->motor_driver_status_msg.data.status.current.low_byte = payload_buffer[1];
msg->motor_driver_status_msg.data.status.rpm.high_byte = payload_buffer[2];
msg->motor_driver_status_msg.data.status.rpm.low_byte = payload_buffer[3];
msg->motor_driver_status_msg.data.status.temperature = payload_buffer[4];
msg->motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->motor_driver_status_msg.data.status.count = frame_cnt;
msg->motor_driver_status_msg.data.status.checksum = frame_checksum;
break;
}
case UART_FRAME_MOTOR4_DRIVER_STATUS_ID:
{
msg->msg_type = ScoutMotorDriverStatusMsg;
msg->motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID;
msg->motor_driver_status_msg.data.status.current.high_byte = payload_buffer[0];
msg->motor_driver_status_msg.data.status.current.low_byte = payload_buffer[1];
msg->motor_driver_status_msg.data.status.rpm.high_byte = payload_buffer[2];
msg->motor_driver_status_msg.data.status.rpm.low_byte = payload_buffer[3];
msg->motor_driver_status_msg.data.status.temperature = payload_buffer[4];
msg->motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->motor_driver_status_msg.data.status.count = frame_cnt;
msg->motor_driver_status_msg.data.status.checksum = frame_checksum;
break;
}
case UART_FRAME_LIGHT_STATUS_ID:
{
msg->msg_type = ScoutLightStatusMsg;
msg->light_status_msg.data.status.light_ctrl_enable = payload_buffer[0];
msg->light_status_msg.data.status.front_light_mode = payload_buffer[1];
msg->light_status_msg.data.status.front_light_custom = payload_buffer[2];
msg->light_status_msg.data.status.rear_light_mode = payload_buffer[3];
msg->light_status_msg.data.status.rear_light_custom = payload_buffer[4];
msg->light_status_msg.data.status.reserved0 = 0x00;
msg->light_status_msg.data.status.count = frame_cnt;
msg->light_status_msg.data.status.checksum = frame_checksum;
break;
}
}
return true;
}

6
scout_base/src/scout_sdk/src/demo/CMakeLists.txt
View File

@@ -1,6 +0,0 @@
# Dependency libraries
#find_package(LIBRARY_NAME REQUIRED)
# tests
add_executable(demo_scout_can demo_scout_can.cpp)
target_link_libraries(demo_scout_can scoutbase)

3
scout_base/src/scout_sdk/src/monitor/app/CMakeLists.txt
View File

@@ -1,3 +0,0 @@
# Add executables
add_executable(scout_monitor app_scout_monitor.cpp)
target_link_libraries(scout_monitor monitor)

31
scout_base/src/scout_sdk/src/monitor/app/app_scout_monitor.cpp
View File

@@ -1,31 +0,0 @@
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <iostream>
#include "monitor/scout_monitor.hpp"
using namespace wescore;
int main(int argc, char **argv)
{
std::string device_name;
if (argc == 2)
{
device_name = {argv[1]};
std::cout << "Specified device: " << device_name << std::endl;
}
else
{
std::cout << "Usage: app_scout_monitor <interface>" << std::endl
<< "Example: ./app_scout_monitor can1" << std::endl;
return -1;
}
ScoutMonitor monitor;
monitor.Run(device_name);
return 0;
}

3
scout_base/src/scout_sdk/src/sdk_core/scout_base/CMakeLists.txt → scout_base/src/scout_sdk/src/scout_base/CMakeLists.txt
View File

@@ -1,11 +1,10 @@
## Add libraries
set(SCOUT_BASE_SRC
src/scout_base.cpp
# src/scout_serial.cpp
)
add_library(scoutbase STATIC ${SCOUT_BASE_SRC})
set_target_properties(scoutbase PROPERTIES POSITION_INDEPENDENT_CODE ON)
target_link_libraries(scoutbase asyncio)
target_link_libraries(scoutbase asyncio scout_protocol)
target_include_directories(scoutbase PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<BUILD_INTERFACE:${Boost_INCLUDE_DIRS}>

110
scout_base/src/scout_sdk/src/scout_base/include/scout_base/scout_base.hpp Normal file
View File

@@ -0,0 +1,110 @@
/*
* scout_base.hpp
*
* Created on: Jun 04, 2019 01:22
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_BASE_HPP
#define SCOUT_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include <functional>
#include "async_io/async_can.hpp"
#include "async_io/async_serial.hpp"
#include "scout_protocol/scout_protocol.h"
#include "scout_protocol/scout_can_parser.h"
#include "scout_protocol/scout_uart_parser.h"
#include "scout_base/scout_types.hpp"
namespace wescore
{
class ScoutBase
{
public:
ScoutBase() = default;
~ScoutBase();
// do not allow copy
ScoutBase(const ScoutBase &scout) = delete;
ScoutBase &operator=(const ScoutBase &scout) = delete;
public:
// connect to roboot from CAN or serial
void Connect(std::string dev_name, int32_t baud_rate = 0);
// 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,
ScoutMotionCmd::FaultClearFlag fault_clr_flag = ScoutMotionCmd::FaultClearFlag::NO_FAULT);
// light control
void SetLightCommand(ScoutLightCmd cmd);
void DisableLightCmdControl();
// get robot state
ScoutState GetScoutState();
private:
// hardware communication interface
std::shared_ptr<ASyncCAN> can_if_;
std::shared_ptr<ASyncSerial> 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_[SCOUT_CMD_BUF_LEN];
// cmd/status update related variables
std::thread cmd_thread_;
std::mutex scout_state_mutex_;
std::mutex motion_cmd_mutex_;
std::mutex light_cmd_mutex_;
ScoutState scout_state_;
ScoutMotionCmd current_motion_cmd_;
ScoutLightCmd 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 ScoutStatusMessage &msg);
public:
static void UpdateScoutState(const ScoutStatusMessage &status_msg, ScoutState &state);
};
} // namespace wescore
#endif /* SCOUT_BASE_HPP */

61
scout_base/src/scout_sdk/src/sdk_core/scout_base/include/scout/scout_command.hpp → scout_base/src/scout_sdk/src/scout_base/include/scout_base/scout_types.hpp
View File

@@ -1,19 +1,62 @@
/*
* scout_command.hpp
* scout_state.hpp
*
* Created on: Jun 11, 2019 10:33
* Created on: Jun 11, 2019 08:48
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_COMMAND_HPP
#define SCOUT_COMMAND_HPP
#ifndef SCOUT_STATE_HPP
#define SCOUT_STATE_HPP
#include <cstdint>
#include <iostream>
namespace wescore
{
struct ScoutState
{
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 ScoutMotionCmd
{
enum class FaultClearFlag
@@ -38,10 +81,10 @@ struct ScoutMotionCmd
int8_t angular_velocity;
FaultClearFlag fault_clear_flag;
static constexpr double max_linear_velocity = 1.5; // 1.5m/s
static constexpr double min_linear_velocity = -1.5; // -1.5m/s
static constexpr double max_angular_velocity = 0.7853; // 0.7853rad/s
static constexpr double min_angular_velocity = -0.7853; // -0.7853rad/s
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 ScoutLightCmd
@@ -65,4 +108,4 @@ struct ScoutLightCmd
};
} // namespace wescore
#endif /* SCOUT_COMMAND_HPP */
#endif /* SCOUT_STATE_HPP */

391
scout_base/src/scout_sdk/src/scout_base/src/scout_base.cpp Normal file
View File

@@ -0,0 +1,391 @@
#include "scout_base/scout_base.hpp"
#include <string>
#include <cstring>
#include <iostream>
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <ratio>
#include <thread>
namespace
{
// source: https://github.com/rxdu/stopwatch
struct StopWatch
{
using Clock = std::chrono::high_resolution_clock;
using time_point = typename Clock::time_point;
using duration = typename Clock::duration;
StopWatch() { tic_point = Clock::now(); };
time_point tic_point;
void tic()
{
tic_point = Clock::now();
};
double toc()
{
return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count() / 1000000.0;
};
// for different precisions
double stoc()
{
return std::chrono::duration_cast<std::chrono::seconds>(Clock::now() - tic_point).count();
};
double mtoc()
{
return std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
};
double utoc()
{
return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
};
double ntoc()
{
return std::chrono::duration_cast<std::chrono::nanoseconds>(Clock::now() - tic_point).count();
};
// you have to call tic() before calling this function
void sleep_until_ms(int64_t period_ms)
{
int64_t duration = period_ms - std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
if (duration > 0)
std::this_thread::sleep_for(std::chrono::milliseconds(duration));
};
void sleep_until_us(int64_t period_us)
{
int64_t duration = period_us - std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
if (duration > 0)
std::this_thread::sleep_for(std::chrono::microseconds(duration));
};
};
} // namespace
namespace wescore
{
ScoutBase::~ScoutBase()
{
if (serial_connected_)
serial_if_->close();
if (cmd_thread_.joinable())
cmd_thread_.join();
}
void ScoutBase::Connect(std::string dev_name, int32_t baud_rate)
{
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 ScoutBase::Disconnect()
{
if (serial_connected_)
{
if (serial_if_->is_open())
serial_if_->close();
}
}
void ScoutBase::ConfigureCANBus(const std::string &can_if_name)
{
can_if_ = std::make_shared<ASyncCAN>(can_if_name);
can_if_->set_receive_callback(std::bind(&ScoutBase::ParseCANFrame, this, std::placeholders::_1));
can_connected_ = true;
}
void ScoutBase::ConfigureSerial(const std::string uart_name, int32_t baud_rate)
{
serial_if_ = std::make_shared<ASyncSerial>(uart_name, baud_rate);
serial_if_->open();
if (serial_if_->is_open())
serial_connected_ = true;
serial_if_->set_receive_callback(std::bind(&ScoutBase::ParseUARTBuffer, this,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3));
}
void ScoutBase::StartCmdThread()
{
current_motion_cmd_.linear_velocity = 0;
current_motion_cmd_.angular_velocity = 0;
current_motion_cmd_.fault_clear_flag = ScoutMotionCmd::FaultClearFlag::NO_FAULT;
cmd_thread_ = std::thread(std::bind(&ScoutBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true;
}
void ScoutBase::SendMotionCmd(uint8_t count)
{
// motion control message
MotionControlMessage m_msg;
if (can_connected_)
m_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
else if (serial_connected_)
m_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
motion_cmd_mutex_.lock();
m_msg.data.cmd.fault_clear_flag = static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
m_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity;
m_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity;
motion_cmd_mutex_.unlock();
m_msg.data.cmd.reserved0 = 0;
m_msg.data.cmd.reserved1 = 0;
m_msg.data.cmd.count = count;
if (can_connected_)
m_msg.data.cmd.checksum = CalcScoutCANChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID, m_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;
EncodeScoutMotionControlMsgToCAN(&m_msg, &m_frame);
can_if_->send_frame(m_frame);
}
else
{
// send to serial port
EncodeMotionControlMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
}
}
void ScoutBase::SendLightCmd(uint8_t count)
{
LightControlMessage l_msg;
light_cmd_mutex_.lock();
if (light_ctrl_enabled_)
{
l_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL;
l_msg.data.cmd.front_light_mode = static_cast<uint8_t>(current_light_cmd_.front_mode);
l_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value;
l_msg.data.cmd.rear_light_mode = static_cast<uint8_t>(current_light_cmd_.rear_mode);
l_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value;
}
else
{
l_msg.data.cmd.light_ctrl_enable = LIGHT_DISABLE_CTRL;
l_msg.data.cmd.front_light_mode = LIGHT_MODE_CONST_OFF;
l_msg.data.cmd.front_light_custom = 0;
l_msg.data.cmd.rear_light_mode = LIGHT_MODE_CONST_OFF;
l_msg.data.cmd.rear_light_custom = 0;
}
light_ctrl_requested_ = false;
light_cmd_mutex_.unlock();
l_msg.data.cmd.reserved0 = 0;
l_msg.data.cmd.count = count;
if (can_connected_)
l_msg.data.cmd.checksum = CalcScoutCANChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, l_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;
EncodeScoutLightControlMsgToCAN(&l_msg, &l_frame);
can_if_->send_frame(l_frame);
}
else
{
// send to serial port
EncodeLightControlMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_);
serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
}
// std::cout << "cmd: " << static_cast<int>(l_msg.data.cmd.front_light_mode) << " , " << static_cast<int>(l_msg.data.cmd.front_light_custom) << " , "
// << static_cast<int>(l_msg.data.cmd.rear_light_mode) << " , " << static_cast<int>(l_msg.data.cmd.rear_light_custom) << std::endl;
// std::cout << "can: ";
// for (int i = 0; i < 8; ++i)
// std::cout << static_cast<int>(l_frame.data[i]) << " ";
// std::cout << "uart: ";
// for (int i = 0; i < tx_cmd_len_; ++i)
// std::cout << static_cast<int>(tx_buffer_[i]) << " ";
// std::cout << std::endl;
}
void ScoutBase::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;
}
}
ScoutState ScoutBase::GetScoutState()
{
std::lock_guard<std::mutex> guard(scout_state_mutex_);
return scout_state_;
}
void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel, ScoutMotionCmd::FaultClearFlag fault_clr_flag)
{
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_)
StartCmdThread();
if (linear_vel < ScoutMotionCmd::min_linear_velocity)
linear_vel = ScoutMotionCmd::min_linear_velocity;
if (linear_vel > ScoutMotionCmd::max_linear_velocity)
linear_vel = ScoutMotionCmd::max_linear_velocity;
if (angular_vel < ScoutMotionCmd::min_angular_velocity)
angular_vel = ScoutMotionCmd::min_angular_velocity;
if (angular_vel > ScoutMotionCmd::max_angular_velocity)
angular_vel = ScoutMotionCmd::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0);
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
}
void ScoutBase::SetLightCommand(ScoutLightCmd cmd)
{
if (!cmd_thread_started_)
StartCmdThread();
std::lock_guard<std::mutex> guard(light_cmd_mutex_);
current_light_cmd_ = cmd;
light_ctrl_enabled_ = true;
light_ctrl_requested_ = true;
}
void ScoutBase::DisableLightCmdControl()
{
std::lock_guard<std::mutex> guard(light_cmd_mutex_);
light_ctrl_enabled_ = false;
light_ctrl_requested_ = true;
}
void ScoutBase::ParseCANFrame(can_frame *rx_frame)
{
// validate checksum, discard frame if fails
if (!rx_frame->data[7] == CalcScoutCANChecksum(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
ScoutStatusMessage status_msg;
DecodeScoutStatusMsgFromCAN(rx_frame, &status_msg);
NewStatusMsgReceivedCallback(status_msg);
}
void ScoutBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
{
// std::cout << "bytes received from serial: " << bytes_received << std::endl;
ScoutStatusMessage status_msg;
for (int i = 0; i < bytes_received; ++i)
{
if (DecodeScoutStatusMsgFromUART(buf[i], &status_msg))
NewStatusMsgReceivedCallback(status_msg);
}
}
void ScoutBase::NewStatusMsgReceivedCallback(const ScoutStatusMessage &msg)
{
// std::cout << "new status msg received" << std::endl;
std::lock_guard<std::mutex> guard(scout_state_mutex_);
UpdateScoutState(msg, scout_state_);
}
void ScoutBase::UpdateScoutState(const ScoutStatusMessage &status_msg, ScoutState &state)
{
switch (status_msg.msg_type)
{
case ScoutMotionStatusMsg:
{
// std::cout << "motion control feedback received" << std::endl;
const MotionStatusMessage &msg = status_msg.motion_status_msg;
state.linear_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.linear_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.linear_velocity.high_byte) << 8) / 1000.0;
state.angular_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.angular_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.angular_velocity.high_byte) << 8) / 1000.0;
break;
}
case ScoutLightStatusMsg:
{
// std::cout << "light control feedback received" << std::endl;
const LightStatusMessage &msg = status_msg.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 ScoutSystemStatusMsg:
{
// std::cout << "system status feedback received" << std::endl;
const SystemStatusMessage &msg = status_msg.system_status_msg;
state.control_mode = msg.data.status.control_mode;
state.base_state = msg.data.status.base_state;
state.battery_voltage = (static_cast<uint16_t>(msg.data.status.battery_voltage.low_byte) | static_cast<uint16_t>(msg.data.status.battery_voltage.high_byte) << 8) / 10.0;
state.fault_code = (static_cast<uint16_t>(msg.data.status.fault_code.low_byte) | static_cast<uint16_t>(msg.data.status.fault_code.high_byte) << 8);
break;
}
case ScoutMotorDriverStatusMsg:
{
// std::cout << "motor 1 driver feedback received" << std::endl;
const MotorDriverStatusMessage &msg = status_msg.motor_driver_status_msg;
for (int i = 0; i < 4; ++i)
{
state.motor_states[status_msg.motor_driver_status_msg.motor_id].current = (static_cast<uint16_t>(msg.data.status.current.low_byte) | static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) / 10.0;
state.motor_states[status_msg.motor_driver_status_msg.motor_id].rpm = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.rpm.low_byte) | static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
state.motor_states[status_msg.motor_driver_status_msg.motor_id].temperature = msg.data.status.temperature;
}
break;
}
}
}
} // namespace wescore

16
scout_base/src/scout_sdk/src/scout_base/tests/CMakeLists.txt Normal file
View File

@@ -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)

39
scout_base/src/scout_sdk/src/scout_base/tests/test_can_msg.cpp Normal file
View File

@@ -0,0 +1,39 @@
#include <iostream>
#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<int>(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;
}

29
scout_base/src/scout_sdk/src/sdk_core/scout_base/tests/test_scout_base.cpp → scout_base/src/scout_sdk/src/scout_base/tests/test_scout_base.cpp
View File

@@ -8,7 +8,7 @@
#include <chrono>
#include <cmath>
#include "scout/scout_base.hpp"
#include "scout_base/scout_base.hpp"
#define TEST_WITHOUT_SERIAL_HARDWARE
@@ -16,9 +16,30 @@ 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 if (argc == 3)
{
device_name = {argv[1]};
baud_rate = std::stol(argv[2]);
std::cout << "Specified serial: " << device_name << "@" << baud_rate << std::endl;
}
else
{
std::cout << "Usage: app_scout_monitor <interface>" << std::endl
<< "Example 1: ./app_scout_monitor can1"
<< "Example 2: ./app_scout_monitor /dev/ttyUSB0 115200" << std::endl;
return -1;
}
ScoutBase scout;
scout.ConnectCANBus("can1");
scout.StartCmdThread(10);
scout.Connect(device_name, baud_rate);
scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0});
@@ -29,7 +50,7 @@ int main(int argc, char **argv)
if(count == 10)
{
// scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_OFF, 0, ScoutLightCmd::LightMode::CONST_OFF, 0});
// scout.SetLightCommand({ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0, ScoutLightCmd::LightMode::LIGHT_MODE_CONST_OFF, 0});
scout.DisableLightCmdControl();
}

33
scout_base/src/scout_sdk/src/demo/demo_scout_can.cpp → scout_base/src/scout_sdk/src/scout_base/tests/test_scout_serial.cpp
View File

@@ -1,31 +1,34 @@
/*
* demo_scout_can.cpp
*
* Created on: Jun 12, 2019 05:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include <iostream>
#include "scout/scout_base.hpp"
#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.ConnectCANBus("can0");
scout.StartCmdThread(10);
// scout.ConfigureCANBus("can1");
scout.Connect("/dev/ttyUSB0", 115200);
// scout.StartCmdThread(10);
scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0});
// scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0});
int count = 0;
while (true)
{
scout.SetMotionCommand(0.5, 0.2);
// scout.SetMotionCommand(0.5, 0.2);
if (count == 10)
scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_OFF, 0, ScoutLightCmd::LightMode::CONST_OFF, 0});
// 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;

51
scout_base/src/scout_sdk/src/scout_base/tests/test_serial_parser.cpp Normal file
View File

@@ -0,0 +1,51 @@
#include <iostream>
#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<int>(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;
}

7
scout_base/src/scout_sdk/src/sdk_core/CMakeLists.txt
View File

@@ -1,7 +0,0 @@
# Add source directories
add_subdirectory(async_io)
add_subdirectory(scout_base)
if(BUILD_TESTS)
add_subdirectory(unit_tests)
endif()

36
scout_base/src/scout_sdk/src/sdk_core/async_io/CMakeLists.txt
View File

@@ -1,36 +0,0 @@
# Dependency libraries
# find_package(LIB_NAME REQUIRED)
# Add libraries
set(ASYNC_IO_LIB_SRC
src/async_serial.cpp
src/async_can.cpp
)
add_library(asyncio STATIC ${ASYNC_IO_LIB_SRC})
set_target_properties(asyncio PROPERTIES POSITION_INDEPENDENT_CODE ON)
target_compile_definitions(asyncio PUBLIC "-DASIO_ENABLE_OLD_SERVICES -DASIO_HAS_POSIX_STREAM_DESCRIPTOR")
target_link_libraries(asyncio asio pthread)
target_include_directories(asyncio PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
# Add executables
if(BUILD_TESTS)
add_subdirectory(tests)
endif()
# # 'make install' to the correct locations (provided by GNUInstallDirs).
# install(TARGETS lib EXPORT MyLibraryConfig
# ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
# LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
# RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) # This is for Windows
# install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
# # This makes the project importable from the install directory
# # Put config file in per-project dir (name MUST match), can also
# # just go into 'cmake'.
# install(EXPORT MyLibraryConfig DESTINATION share/MyLibrary/cmake)
# # This makes the project importable from the build directory
# export(TARGETS lib FILE MyLibraryConfig.cmake)

64
scout_base/src/scout_sdk/src/sdk_core/async_io/tests/test_aserial.cpp
View File

@@ -1,64 +0,0 @@
/*
* test_interface.cpp
*
* Created on: Dec 25, 2016
* Author: rdu
*/
#include <iostream>
#include "async_io/async_serial.hpp"
using namespace wescore;
void parse_buffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
{
std::cout << "parser called" << std::endl;
// mavlink::mavlink_status_t status;
// mavlink::mavlink_message_t message;
for (; bytes_received > 0; bytes_received--)
{
auto c = *buf++;
// // based on mavlink_parse_char()
// auto msg_received = static_cast<Framing>(mavlink::mavlink_frame_char_buffer(&m_buffer, &m_status, c, &message, &status));
// if (msg_received == Framing::bad_crc || msg_received == Framing::bad_signature) {
// mavlink::_mav_parse_error(&m_status);
// m_status.msg_received = mavlink::MAVLINK_FRAMING_INCOMPLETE;
// m_status.parse_state = mavlink::MAVLINK_PARSE_STATE_IDLE;
// if (c == MAVLINK_STX) {
// m_status.parse_state = mavlink::MAVLINK_PARSE_STATE_GOT_STX;
// m_buffer.len = 0;
// mavlink::mavlink_start_checksum(&m_buffer);
// }
// }
// if (msg_received != Framing::incomplete) {
// log_recv(pfx, message, msg_received);
// if (message_received_cb)
// message_received_cb(&message, msg_received);
// }
}
}
int main(int argc, char *argv[])
{
// ASyncSerial::Ptr serial = ASyncSerial::open_url("/dev/ttyUSB0:115200");
std::shared_ptr<ASyncSerial> serial = std::make_shared<ASyncSerial>("/dev/pts/6", 115200);
serial->set_receive_callback(parse_buffer);
if (serial->is_open())
std::cout << "serial port opened" << std::endl;
uint8_t data[8] = {'a','b','c'};
while (1)
{
serial->send_bytes(data, 3);
sleep(1);
}
}

76
scout_base/src/scout_sdk/src/sdk_core/scout_base/include/scout/scout_base.hpp
View File

@@ -1,76 +0,0 @@
/*
* scout_base.hpp
*
* Created on: Jun 04, 2019 01:22
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_BASE_HPP
#define SCOUT_BASE_HPP
#include <string>
#include <cstdint>
#include <thread>
#include <mutex>
#include <functional>
#include "async_io/async_can.hpp"
#include "scout/scout_state.hpp"
#include "scout/scout_command.hpp"
namespace wescore
{
class ScoutBase
{
public:
ScoutBase() = default;
~ScoutBase();
// do not allow copy
ScoutBase(const ScoutBase &scout) = delete;
ScoutBase &operator=(const ScoutBase &scout) = delete;
public:
void ConnectCANBus(const std::string &can_if_name = "can1");
void StartCmdThread(int32_t period_ms);
// motion control
void SetMotionCommand(double linear_vel, double angular_vel,
ScoutMotionCmd::FaultClearFlag fault_clr_flag = ScoutMotionCmd::FaultClearFlag::NO_FAULT);
// light control
void SetLightCommand(ScoutLightCmd cmd);
void DisableLightCmdControl();
// get robot state
ScoutState GetScoutState();
// TODO internal use only, for testing, will be set private in future release
void UpdateScoutState(ScoutState &state, can_frame *rx_frame);
private:
std::shared_ptr<ASyncCAN> can_if_;
std::thread cmd_thread_;
std::mutex scout_state_mutex_;
std::mutex motion_cmd_mutex_;
std::mutex light_cmd_mutex_;
ScoutState scout_state_;
ScoutMotionCmd current_motion_cmd_;
ScoutLightCmd current_light_cmd_;
bool cmd_thread_started_ = false;
bool light_ctrl_enabled_ = false;
bool light_ctrl_requested_ = false;
void ControlLoop(int32_t period_ms);
void ParseCANFrame(can_frame *rx_frame);
};
} // namespace wescore
#endif /* SCOUT_BASE_HPP */

337
scout_base/src/scout_sdk/src/sdk_core/scout_base/include/scout/scout_can_protocol.h
View File

@@ -1,337 +0,0 @@
/*
* scout_can_protocol.h
*
* Created on: Jun 10, 2019 23:23
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_CAN_PROTOCOL_H
#define SCOUT_CAN_PROTOCOL_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stdint.h"
enum ScoutCANMsgIDs
{
MSG_MOTION_CONTROL_ID = 0x130,
MSG_MOTION_CONTROL_FEEDBACK_ID = 0x131,
MSG_LIGHT_CONTROL_ID = 0x140,
MSG_LIGHT_CONTROL_FEEDBACK_ID = 0x141,
MSG_SYSTEM_STATUS_FEEDBACK_ID = 0x151,
MSG_MOTOR1_DRIVER_FEEDBACK_ID = 0x200,
MSG_MOTOR2_DRIVER_FEEDBACK_ID = 0x201,
MSG_MOTOR3_DRIVER_FEEDBACK_ID = 0x202,
MSG_MOTOR4_DRIVER_FEEDBACK_ID = 0x203,
MSG_LAST_ID
};
static uint8_t Agilex_CANMsgChecksum(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;
};
/*------------------------- Motion Control Message -------------------------*/
enum ControlMode
{
REMOTE_MODE = 0x00,
CMD_CAN_MODE = 0x01,
CMD_UART_MODE = 0x02
};
enum 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
};
typedef struct
{
const uint16_t id = MSG_MOTION_CONTROL_ID;
const uint8_t dlc = 0x08;
union {
struct CmdDef
{
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;
/*-------------------------- Light Control Message -------------------------*/
enum LightControlFlag
{
DISABLE_LIGHT_CTRL = 0x00,
ENABLE_LIGHT_CTRL = 0x01
};
enum LightMode
{
CONST_OFF = 0x00,
CONST_ON = 0x01,
BREATH = 0x02,
CUSTOM = 0x03
};
typedef struct
{
const uint16_t id = MSG_LIGHT_CONTROL_ID;
const uint8_t dlc = 0x08;
union {
struct CmdDef
{
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;
/*--------------------- Motion Control Feedback Message --------------------*/
typedef struct
{
const uint16_t id = MSG_MOTION_CONTROL_FEEDBACK_ID;
const uint8_t dlc = 0x08;
union {
struct StatusDef
{
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;
/*---------------------- Light Control Feedback Message --------------------*/
typedef struct
{
const uint16_t id = MSG_LIGHT_CONTROL_FEEDBACK_ID;
const uint8_t dlc = 0x08;
union {
struct StatusDef
{
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;
/*---------------------- System Status Feedback Message --------------------*/
enum BaseState
{
BASE_NORMAL,
BASE_ESTOP,
BASE_EXCEPTION
};
enum FaultBits
{
CAN_CHECKSUM_ERROR = 0x0100,
MOTOR_DRV_OVERHEAT_W = 0x0200,
MOTOR_OVERCURRENT_W = 0x0400,
BAT_UNDER_VOL_W = 0x0800,
BAT_UNDER_VOL_F = 0x01,
BAT_OVER_VOL_F = 0x02,
MOTOR1_COMM_F = 0x04,
MOTOR2_COMM_F = 0x08,
MOTOR3_COMM_F = 0x10,
MOTOR4_COMM_F = 0x20,
MOTOR_DRV_OVERHEAT_F = 0x40,
MOTOR_OVERCURRENT_F = 0x80
};
typedef struct
{
const uint16_t id = MSG_SYSTEM_STATUS_FEEDBACK_ID;
const uint8_t dlc = 0x08;
union {
struct StatusDef
{
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;
/*--------------------- Motor 1 Driver Feedback Message --------------------*/
typedef struct
{
const uint16_t id = MSG_MOTOR1_DRIVER_FEEDBACK_ID;
const uint8_t dlc = 0x08;
union {
struct StatusDef
{
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;
} Motor1DriverStatusMessage;
/*--------------------- Motor 2 Driver Feedback Message --------------------*/
typedef struct
{
const uint16_t id = MSG_MOTOR2_DRIVER_FEEDBACK_ID;
const uint8_t dlc = 0x08;
union {
struct StatusDef
{
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;
} Motor2DriverStatusMessage;
/*--------------------- Motor 3 Driver Feedback Message --------------------*/
typedef struct
{
const uint16_t id = MSG_MOTOR3_DRIVER_FEEDBACK_ID;
const uint8_t dlc = 0x08;
union {
struct StatusDef
{
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;
} Motor3DriverStatusMessage;
/*--------------------- Motor 4 Driver Feedback Message --------------------*/
typedef struct
{
const uint16_t id = MSG_MOTOR4_DRIVER_FEEDBACK_ID;
const uint8_t dlc = 0x08;
union {
struct StatusDef
{
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;
} Motor4DriverStatusMessage;
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_CAN_PROTOCOL_H */

21
scout_base/src/scout_sdk/src/sdk_core/scout_base/include/scout/scout_io.hpp
View File

@@ -1,21 +0,0 @@
/*
* scout_io.hpp
*
* Created on: May 04, 2019 22:04
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_IO_HPP
#define SCOUT_IO_HPP
namespace scout
{
class ScoutIO
{
public:
};
} // namespace scout
#endif /* SCOUT_IO_HPP */

66
scout_base/src/scout_sdk/src/sdk_core/scout_base/include/scout/scout_serial.hpp
View File

@@ -1,66 +0,0 @@
/*
* scout_serial.h
*
* Created on: May 05, 2019 11:35
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_SERIAL_H
#define SCOUT_SERIAL_H
#include "scout_io/scout_io.h"
namespace scout
{
struct Frame_t
{
unsigned short Header;
unsigned char Len;
unsigned char Typedef;
unsigned char Count;
unsigned char Time_Out;
short Linear;
short Angular;
unsigned short CheckSum;
};
struct Cmd_t
{
unsigned short Linear;
unsigned short Angular;
bool IsUpdata;
};
typedef enum
{
eHead = 0,
eLen = 1,
eTypedef = 3,
eChecksum = 2
} state_t;
class ScoutSerial : public ScoutIO
{
public:
Cmd_t Get_dataOfTransport();
void Set_dataOfTransport(Cmd_t *CMD);
void Read_DataOfChassis_Loop(void);
void Send_SpeedToChassis(short Angular, short Linear, unsigned char Count);
void Send_Speed(short Angular, short Linear, unsigned char Count);
unsigned short Checksum(unsigned char *data, unsigned short len);
void Find_NextHead();
};
Cmd_t Get_dataOfTransport();
void Set_dataOfTransport(Cmd_t *CMD);
void Read_DataOfChassis_Loop(void);
void Send_SpeedToChassis(short Angular, short Linear, unsigned char Count);
void Send_Speed(short Angular, short Linear, unsigned char Count);
unsigned short Checksum(unsigned char *data, unsigned short len);
void Find_NextHead();
void Send_SpeedToChassis(short Angular, short Linear, unsigned char Count);
} // namespace scout
#endif /* SCOUT_SERIAL_H */

36
scout_base/src/scout_sdk/src/sdk_core/scout_base/include/scout/scout_serial_protocol.h
View File

@@ -1,36 +0,0 @@
/*
* scout_serial_protocol.hpp
*
* Created on: Jun 05, 2019 02:34
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_SERIAL_PROTOCOL_HPP
#define SCOUT_SERIAL_PROTOCOL_HPP
#ifdef __cplusplus
extern "C" {
#endif
struct ScoutSerialProtocol
{
struct Frame
{
unsigned short Header;
unsigned char Len;
unsigned char Typedef;
unsigned char Count;
unsigned char Time_Out;
short Linear;
short Angular;
unsigned short CheckSum;
};
};
#ifdef __cplusplus
}
#endif
#endif /* SCOUT_SERIAL_PROTOCOL_HPP */

61
scout_base/src/scout_sdk/src/sdk_core/scout_base/include/scout/scout_state.hpp
View File

@@ -1,61 +0,0 @@
/*
* scout_state.hpp
*
* Created on: Jun 11, 2019 08:48
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_STATE_HPP
#define SCOUT_STATE_HPP
#include <cstdint>
#include <iostream>
namespace wescore
{
struct ScoutState
{
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;
double angular_velocity;
};
} // namespace wescore
#endif /* SCOUT_STATE_HPP */

316
scout_base/src/scout_sdk/src/sdk_core/scout_base/src/scout_base.cpp
View File

@@ -1,316 +0,0 @@
#include "scout/scout_base.hpp"
#include <string>
#include <cstring>
#include <iostream>
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <ratio>
#include <thread>
#include "scout/scout_can_protocol.h"
namespace
{
// source: https://github.com/rxdu/stopwatch
struct StopWatch
{
using Clock = std::chrono::high_resolution_clock;
using time_point = typename Clock::time_point;
using duration = typename Clock::duration;
StopWatch() { tic_point = Clock::now(); };
time_point tic_point;
void tic()
{
tic_point = Clock::now();
};
double toc()
{
return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count() / 1000000.0;
};
// for different precisions
double stoc()
{
return std::chrono::duration_cast<std::chrono::seconds>(Clock::now() - tic_point).count();
};
double mtoc()
{
return std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
};
double utoc()
{
return std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
};
double ntoc()
{
return std::chrono::duration_cast<std::chrono::nanoseconds>(Clock::now() - tic_point).count();
};
// you have to call tic() before calling this function
void sleep_until_ms(int64_t period_ms)
{
int64_t duration = period_ms - std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - tic_point).count();
if (duration > 0)
std::this_thread::sleep_for(std::chrono::milliseconds(duration));
};
void sleep_until_us(int64_t period_us)
{
int64_t duration = period_us - std::chrono::duration_cast<std::chrono::microseconds>(Clock::now() - tic_point).count();
if (duration > 0)
std::this_thread::sleep_for(std::chrono::microseconds(duration));
};
};
} // namespace
namespace wescore
{
ScoutBase::~ScoutBase()
{
if (cmd_thread_.joinable())
cmd_thread_.join();
}
void ScoutBase::ConnectCANBus(const std::string &can_if_name)
{
can_if_ = std::make_shared<ASyncCAN>(can_if_name);
can_if_->set_receive_callback(std::bind(&ScoutBase::ParseCANFrame, this, std::placeholders::_1));
}
void ScoutBase::StartCmdThread(int32_t period_ms)
{
cmd_thread_ = std::thread(std::bind(&ScoutBase::ControlLoop, this, period_ms));
cmd_thread_started_ = true;
}
void ScoutBase::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
{
MotionControlMessage m_msg;
m_msg.data.cmd.control_mode = CMD_CAN_MODE;
motion_cmd_mutex_.lock();
m_msg.data.cmd.fault_clear_flag = static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
m_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity;
m_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity;
motion_cmd_mutex_.unlock();
m_msg.data.cmd.reserved0 = 0;
m_msg.data.cmd.reserved1 = 0;
m_msg.data.cmd.count = cmd_count++;
m_msg.data.cmd.checksum = Agilex_CANMsgChecksum(m_msg.id, m_msg.data.raw, m_msg.dlc);
// send to can bus
can_frame m_frame;
m_frame.can_id = m_msg.id;
m_frame.can_dlc = m_msg.dlc;
std::memcpy(m_frame.data, m_msg.data.raw, m_msg.dlc * sizeof(uint8_t));
can_if_->send_frame(m_frame);
}
// check if there is request for light control
if (light_ctrl_requested_)
{
LightControlMessage l_msg;
light_cmd_mutex_.lock();
if (light_ctrl_enabled_)
{
l_msg.data.cmd.light_ctrl_enable = ENABLE_LIGHT_CTRL;
l_msg.data.cmd.front_light_mode = static_cast<uint8_t>(current_light_cmd_.front_mode);
l_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value;
l_msg.data.cmd.rear_light_mode = static_cast<uint8_t>(current_light_cmd_.rear_mode);
l_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value;
}
else
{
l_msg.data.cmd.light_ctrl_enable = DISABLE_LIGHT_CTRL;
l_msg.data.cmd.front_light_mode = CONST_OFF;
l_msg.data.cmd.front_light_custom = 0;
l_msg.data.cmd.rear_light_mode = CONST_OFF;
l_msg.data.cmd.rear_light_custom = 0;
}
light_ctrl_requested_ = false;
light_cmd_mutex_.unlock();
l_msg.data.cmd.reserved0 = 0;
l_msg.data.cmd.count = light_cmd_count++;
l_msg.data.cmd.checksum = Agilex_CANMsgChecksum(l_msg.id, l_msg.data.raw, l_msg.dlc);
can_frame l_frame;
l_frame.can_id = l_msg.id;
l_frame.can_dlc = l_msg.dlc;
std::memcpy(l_frame.data, l_msg.data.raw, l_msg.dlc * sizeof(uint8_t));
can_if_->send_frame(l_frame);
}
ctrl_sw.sleep_until_ms(period_ms);
// std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << std::endl;
}
}
ScoutState ScoutBase::GetScoutState()
{
std::lock_guard<std::mutex> guard(scout_state_mutex_);
return scout_state_;
}
void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel, ScoutMotionCmd::FaultClearFlag fault_clr_flag)
{
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_)
StartCmdThread(10);
if (linear_vel < ScoutMotionCmd::min_linear_velocity)
linear_vel = ScoutMotionCmd::min_linear_velocity;
if (linear_vel > ScoutMotionCmd::max_linear_velocity)
linear_vel = ScoutMotionCmd::max_linear_velocity;
if (angular_vel < ScoutMotionCmd::min_angular_velocity)
angular_vel = ScoutMotionCmd::min_angular_velocity;
if (angular_vel > ScoutMotionCmd::max_angular_velocity)
angular_vel = ScoutMotionCmd::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<uint8_t>(linear_vel / ScoutMotionCmd::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<uint8_t>(angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0);
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
}
void ScoutBase::SetLightCommand(ScoutLightCmd cmd)
{
std::lock_guard<std::mutex> guard(light_cmd_mutex_);
current_light_cmd_ = cmd;
light_ctrl_enabled_ = true;
light_ctrl_requested_ = true;
}
void ScoutBase::DisableLightCmdControl()
{
std::lock_guard<std::mutex> guard(light_cmd_mutex_);
light_ctrl_enabled_ = false;
light_ctrl_requested_ = true;
}
void ScoutBase::ParseCANFrame(can_frame *rx_frame)
{
// validate checksum, discard frame if fails
if (!rx_frame->data[7] == Agilex_CANMsgChecksum(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
std::lock_guard<std::mutex> guard(scout_state_mutex_);
UpdateScoutState(scout_state_, rx_frame);
}
void ScoutBase::UpdateScoutState(ScoutState &state, can_frame *rx_frame)
{
switch (rx_frame->can_id)
{
case MSG_MOTION_CONTROL_FEEDBACK_ID:
{
// std::cout << "motion control feedback received" << std::endl;
MotionStatusMessage msg;
std::memcpy(msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
state.linear_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.linear_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.linear_velocity.high_byte) << 8) / 1000.0;
state.angular_velocity = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.angular_velocity.low_byte) | static_cast<uint16_t>(msg.data.status.angular_velocity.high_byte) << 8) / 1000.0;
break;
}
case MSG_LIGHT_CONTROL_FEEDBACK_ID:
{
// std::cout << "light control feedback received" << std::endl;
LightStatusMessage msg;
std::memcpy(msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
if (msg.data.status.light_ctrl_enable == DISABLE_LIGHT_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 MSG_SYSTEM_STATUS_FEEDBACK_ID:
{
// std::cout << "system status feedback received" << std::endl;
SystemStatusMessage msg;
std::memcpy(msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
state.control_mode = msg.data.status.control_mode;
state.base_state = msg.data.status.base_state;
state.battery_voltage = (static_cast<uint16_t>(msg.data.status.battery_voltage.low_byte) | static_cast<uint16_t>(msg.data.status.battery_voltage.high_byte) << 8) / 10.0;
state.fault_code = (static_cast<uint16_t>(msg.data.status.fault_code.low_byte) | static_cast<uint16_t>(msg.data.status.fault_code.high_byte) << 8);
break;
}
case MSG_MOTOR1_DRIVER_FEEDBACK_ID:
{
// std::cout << "motor 1 driver feedback received" << std::endl;
Motor1DriverStatusMessage msg;
std::memcpy(msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
state.motor_states[0].current = (static_cast<uint16_t>(msg.data.status.current.low_byte) | static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) / 10.0;
state.motor_states[0].rpm = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.rpm.low_byte) | static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
;
state.motor_states[0].temperature = msg.data.status.temperature;
break;
}
case MSG_MOTOR2_DRIVER_FEEDBACK_ID:
{
// std::cout << "motor 2 driver feedback received" << std::endl;
Motor2DriverStatusMessage msg;
std::memcpy(msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
state.motor_states[1].current = (static_cast<uint16_t>(msg.data.status.current.low_byte) | static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) / 10.0;
state.motor_states[1].rpm = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.rpm.low_byte) | static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
;
state.motor_states[1].temperature = msg.data.status.temperature;
break;
}
case MSG_MOTOR3_DRIVER_FEEDBACK_ID:
{
// std::cout << "motor 3 driver feedback received" << std::endl;
Motor3DriverStatusMessage msg;
std::memcpy(msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
state.motor_states[2].current = (static_cast<uint16_t>(msg.data.status.current.low_byte) | static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) / 10.0;
state.motor_states[2].rpm = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.rpm.low_byte) | static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
;
state.motor_states[2].temperature = msg.data.status.temperature;
break;
}
case MSG_MOTOR4_DRIVER_FEEDBACK_ID:
{
// std::cout << "motor 4 driver feedback received" << std::endl;
Motor4DriverStatusMessage msg;
std::memcpy(msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
state.motor_states[3].current = (static_cast<uint16_t>(msg.data.status.current.low_byte) | static_cast<uint16_t>(msg.data.status.current.high_byte) << 8) / 10.0;
state.motor_states[3].rpm = static_cast<int16_t>(static_cast<uint16_t>(msg.data.status.rpm.low_byte) | static_cast<uint16_t>(msg.data.status.rpm.high_byte) << 8);
state.motor_states[3].temperature = msg.data.status.temperature;
break;
}
}
}
} // namespace wescore

9
scout_base/src/scout_sdk/src/sdk_core/scout_base/tests/CMakeLists.txt
View File

@@ -1,9 +0,0 @@
# 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_can_msg test_can_msg.cpp)
target_link_libraries(test_can_msg scoutbase)

37
scout_base/src/scout_sdk/src/sdk_core/scout_base/tests/test_can_msg.cpp
View File

@@ -1,37 +0,0 @@
#include <iostream>
#include "scout/scout_can_protocol.h"
void print_msg(uint8_t data[8])
{
for (int i = 0; i < 8; ++i)
std::cout << std::hex << static_cast<int>(data[i]) << " ";
std::cout << std::dec << std::endl;
}
int main()
{
MotionControlMessage msg;
msg.data.cmd.control_mode = CMD_CAN_MODE;
msg.data.cmd.fault_clear_flag = NO_FAULT;
msg.data.cmd.linear_velocity_cmd = 10;
msg.data.cmd.angular_velocity_cmd = 0;
msg.data.cmd.reserved0 = 0;
msg.data.cmd.reserved1 = 0;
msg.data.cmd.count = 0;
msg.data.cmd.checksum = Agilex_CANMsgChecksum(msg.id, msg.data.raw, msg.dlc);
print_msg(msg.data.raw);
LightControlMessage msg2;
msg2.data.cmd.light_ctrl_enable = DISABLE_LIGHT_CTRL;
msg2.data.cmd.front_light_mode = CONST_ON;
msg2.data.cmd.front_light_custom = 0;
msg2.data.cmd.rear_light_mode = CONST_ON;
msg2.data.cmd.rear_light_custom = 0;
msg2.data.cmd.reserved0 = 0;
msg2.data.cmd.count = 0;
msg2.data.cmd.checksum = Agilex_CANMsgChecksum(msg2.id, msg2.data.raw, msg2.dlc);
print_msg(msg2.data.raw);
return 0;
}

0
scout_base/src/scout_sdk/src/sdk_core/unit_tests/CMakeLists.txt → scout_base/src/scout_sdk/src/unit_tests/CMakeLists.txt
View File

73
scout_base/src/scout_sdk/src/sdk_core/unit_tests/gtests/scout_can_protocol_test.cpp → scout_base/src/scout_sdk/src/unit_tests/gtests/scout_can_protocol_test.cpp
View File

@@ -15,8 +15,7 @@
#include "gtest/gtest.h"
#include "scout/scout_base.hpp"
#include "scout/scout_can_protocol.h"
#include "scout_base/scout_base.hpp"
using namespace wescore;
@@ -25,7 +24,7 @@ struct ScoutCANProtocolTest : testing::Test
ScoutCANProtocolTest()
{
// control frames
motion_ctrl_frame.can_id = MSG_MOTION_CONTROL_ID;
motion_ctrl_frame.can_id = CAN_MSG_MOTION_CONTROL_CMD_ID;
motion_ctrl_frame.can_dlc = 8;
motion_ctrl_frame.data[0] = 0x01;
motion_ctrl_frame.data[1] = 0x00;
@@ -34,11 +33,11 @@ struct ScoutCANProtocolTest : testing::Test
motion_ctrl_frame.data[4] = 0x00;
motion_ctrl_frame.data[5] = 0x00;
motion_ctrl_frame.data[6] = 0x00;
motion_ctrl_frame.data[7] = Agilex_CANMsgChecksum(motion_ctrl_frame.can_id,
motion_ctrl_frame.data[7] = CalcScoutCANChecksum(motion_ctrl_frame.can_id,
motion_ctrl_frame.data,
motion_ctrl_frame.can_dlc);
light_ctrl_frame.can_id = MSG_LIGHT_CONTROL_ID;
light_ctrl_frame.can_id = CAN_MSG_LIGHT_CONTROL_CMD_ID;
light_ctrl_frame.can_dlc = 8;
light_ctrl_frame.data[0] = 0x01;
light_ctrl_frame.data[1] = 0x00;
@@ -47,12 +46,12 @@ struct ScoutCANProtocolTest : testing::Test
light_ctrl_frame.data[4] = 0x00;
light_ctrl_frame.data[5] = 0x00;
light_ctrl_frame.data[6] = 0x00;
light_ctrl_frame.data[7] = Agilex_CANMsgChecksum(light_ctrl_frame.can_id,
light_ctrl_frame.data[7] = CalcScoutCANChecksum(light_ctrl_frame.can_id,
light_ctrl_frame.data,
light_ctrl_frame.can_dlc);
// feedback frames
motion_status_frame.can_id = MSG_MOTION_CONTROL_FEEDBACK_ID;
motion_status_frame.can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
motion_status_frame.can_dlc = 8;
motion_status_frame.data[0] = 0x04;
motion_status_frame.data[1] = 0xe8; // 1.256
@@ -61,11 +60,11 @@ struct ScoutCANProtocolTest : testing::Test
motion_status_frame.data[4] = 0x00;
motion_status_frame.data[5] = 0x00;
motion_status_frame.data[6] = 0x00;
motion_status_frame.data[7] = Agilex_CANMsgChecksum(motion_status_frame.can_id,
motion_status_frame.data[7] = CalcScoutCANChecksum(motion_status_frame.can_id,
motion_status_frame.data,
motion_status_frame.can_dlc);
motion_status_frame2.can_id = MSG_MOTION_CONTROL_FEEDBACK_ID;
motion_status_frame2.can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
motion_status_frame2.can_dlc = 8;
motion_status_frame2.data[0] = 0xfb;
motion_status_frame2.data[1] = 0x18; // -1.256
@@ -74,11 +73,11 @@ struct ScoutCANProtocolTest : testing::Test
motion_status_frame2.data[4] = 0x00;
motion_status_frame2.data[5] = 0x00;
motion_status_frame2.data[6] = 0x00;
motion_status_frame2.data[7] = Agilex_CANMsgChecksum(motion_status_frame2.can_id,
motion_status_frame2.data[7] = CalcScoutCANChecksum(motion_status_frame2.can_id,
motion_status_frame2.data,
motion_status_frame2.can_dlc);
light_status_frame.can_id = MSG_LIGHT_CONTROL_FEEDBACK_ID;
light_status_frame.can_id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
light_status_frame.can_dlc = 8;
light_status_frame.data[0] = 0x01;
light_status_frame.data[1] = 0x03;
@@ -87,11 +86,11 @@ struct ScoutCANProtocolTest : testing::Test
light_status_frame.data[4] = 0x32; // 50
light_status_frame.data[5] = 0x00;
light_status_frame.data[6] = 0x00;
light_status_frame.data[7] = Agilex_CANMsgChecksum(light_status_frame.can_id,
light_status_frame.data[7] = CalcScoutCANChecksum(light_status_frame.can_id,
light_status_frame.data,
light_status_frame.can_dlc);
system_status_frame.can_id = MSG_SYSTEM_STATUS_FEEDBACK_ID;
system_status_frame.can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
system_status_frame.can_dlc = 8;
system_status_frame.data[0] = 0x01;
system_status_frame.data[1] = 0x01;
@@ -100,11 +99,11 @@ struct ScoutCANProtocolTest : testing::Test
system_status_frame.data[4] = 0xf0;
system_status_frame.data[5] = 0xff;
system_status_frame.data[6] = 0x00;
system_status_frame.data[7] = Agilex_CANMsgChecksum(system_status_frame.can_id,
system_status_frame.data[7] = CalcScoutCANChecksum(system_status_frame.can_id,
system_status_frame.data,
system_status_frame.can_dlc);
motor1_driver_status_frame.can_id = MSG_MOTOR1_DRIVER_FEEDBACK_ID;
motor1_driver_status_frame.can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
motor1_driver_status_frame.can_dlc = 8;
motor1_driver_status_frame.data[0] = 0x00;
motor1_driver_status_frame.data[1] = 0x7d; // 12.5
@@ -113,28 +112,28 @@ struct ScoutCANProtocolTest : testing::Test
motor1_driver_status_frame.data[4] = 0x38; // 56
motor1_driver_status_frame.data[5] = 0x00;
motor1_driver_status_frame.data[6] = 0x00;
motor1_driver_status_frame.data[7] = Agilex_CANMsgChecksum(motor1_driver_status_frame.can_id,
motor1_driver_status_frame.data[7] = CalcScoutCANChecksum(motor1_driver_status_frame.can_id,
motor1_driver_status_frame.data,
motor1_driver_status_frame.can_dlc);
motor2_driver_status_frame = motor1_driver_status_frame;
motor2_driver_status_frame.can_id = MSG_MOTOR2_DRIVER_FEEDBACK_ID;
motor2_driver_status_frame.can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
motor2_driver_status_frame.data[2] = 0xfb;
motor2_driver_status_frame.data[3] = 0x2e; // -1234
motor2_driver_status_frame.data[4] = 0xc8; // -56
motor2_driver_status_frame.data[7] = Agilex_CANMsgChecksum(motor2_driver_status_frame.can_id,
motor2_driver_status_frame.data[7] = CalcScoutCANChecksum(motor2_driver_status_frame.can_id,
motor2_driver_status_frame.data,
motor2_driver_status_frame.can_dlc);
motor3_driver_status_frame = motor1_driver_status_frame;
motor3_driver_status_frame.can_id = MSG_MOTOR3_DRIVER_FEEDBACK_ID;
motor3_driver_status_frame.data[7] = Agilex_CANMsgChecksum(motor3_driver_status_frame.can_id,
motor3_driver_status_frame.can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
motor3_driver_status_frame.data[7] = CalcScoutCANChecksum(motor3_driver_status_frame.can_id,
motor3_driver_status_frame.data,
motor3_driver_status_frame.can_dlc);
motor4_driver_status_frame = motor2_driver_status_frame;
motor4_driver_status_frame.can_id = MSG_MOTOR4_DRIVER_FEEDBACK_ID;
motor4_driver_status_frame.data[7] = Agilex_CANMsgChecksum(motor4_driver_status_frame.can_id,
motor4_driver_status_frame.can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
motor4_driver_status_frame.data[7] = CalcScoutCANChecksum(motor4_driver_status_frame.can_id,
motor4_driver_status_frame.data,
motor4_driver_status_frame.can_dlc);
}
@@ -158,13 +157,17 @@ struct ScoutCANProtocolTest : testing::Test
TEST_F(ScoutCANProtocolTest, MotionStatusMsg)
{
ScoutState state;
scout_base.UpdateScoutState(state, &motion_status_frame);
ScoutStatusMessage msg;
DecodeScoutStatusMsgFromCAN(&motion_status_frame, &msg);
ScoutBase::UpdateScoutState(msg, state);
ASSERT_FLOAT_EQ(state.linear_velocity, 1.256);
ASSERT_FLOAT_EQ(state.angular_velocity, 0.123);
ScoutState state2;
scout_base.UpdateScoutState(state2, &motion_status_frame2);
ScoutStatusMessage msg2;
DecodeScoutStatusMsgFromCAN(&motion_status_frame2, &msg2);
ScoutBase::UpdateScoutState(msg2, state2);
ASSERT_FLOAT_EQ(state2.linear_velocity, -1.256);
ASSERT_FLOAT_EQ(state2.angular_velocity, -0.123);
@@ -173,7 +176,9 @@ TEST_F(ScoutCANProtocolTest, MotionStatusMsg)
TEST_F(ScoutCANProtocolTest, LightStatusMsg)
{
ScoutState state;
scout_base.UpdateScoutState(state, &light_status_frame);
ScoutStatusMessage msg;
DecodeScoutStatusMsgFromCAN(&light_status_frame, &msg);
ScoutBase::UpdateScoutState(msg, state);
ASSERT_EQ(state.light_control_enabled, true);
ASSERT_EQ(state.front_light_state.mode, 0x03);
@@ -185,7 +190,9 @@ TEST_F(ScoutCANProtocolTest, LightStatusMsg)
TEST_F(ScoutCANProtocolTest, SystemStatusMsg)
{
ScoutState state;
scout_base.UpdateScoutState(state, &system_status_frame);
ScoutStatusMessage msg;
DecodeScoutStatusMsgFromCAN(&system_status_frame, &msg);
ScoutBase::UpdateScoutState(msg, state);
ASSERT_EQ(state.base_state, true);
ASSERT_EQ(state.control_mode, 0x01);
@@ -196,10 +203,15 @@ TEST_F(ScoutCANProtocolTest, SystemStatusMsg)
TEST_F(ScoutCANProtocolTest, MotorDriverStatusMsg)
{
ScoutState state;
scout_base.UpdateScoutState(state, &motor1_driver_status_frame);
scout_base.UpdateScoutState(state, &motor2_driver_status_frame);
scout_base.UpdateScoutState(state, &motor3_driver_status_frame);
scout_base.UpdateScoutState(state, &motor4_driver_status_frame);
ScoutStatusMessage msg1, msg2, msg3, msg4;
DecodeScoutStatusMsgFromCAN(&motor1_driver_status_frame, &msg1);
DecodeScoutStatusMsgFromCAN(&motor2_driver_status_frame, &msg2);
DecodeScoutStatusMsgFromCAN(&motor3_driver_status_frame, &msg3);
DecodeScoutStatusMsgFromCAN(&motor4_driver_status_frame, &msg4);
ScoutBase::UpdateScoutState(msg1, state);
ScoutBase::UpdateScoutState(msg2, state);
ScoutBase::UpdateScoutState(msg3, state);
ScoutBase::UpdateScoutState(msg4, state);
ASSERT_FLOAT_EQ(state.motor_states[0].current, 12.5);
ASSERT_FLOAT_EQ(state.motor_states[0].rpm, 1234);
@@ -217,4 +229,3 @@ TEST_F(ScoutCANProtocolTest, MotorDriverStatusMsg)
ASSERT_FLOAT_EQ(state.motor_states[3].rpm, -1234);
ASSERT_FLOAT_EQ(state.motor_states[3].temperature, -56);
}