mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

major update to repo structure

Browse Source
This commit is contained in:
Ruixiang Du
2020-10-23 10:44:15 +08:00
parent f981dc4f36
commit 1bfa475ac0
5253 changed files with 302 additions and 757923 deletions
Download Patch File Download Diff File Expand all files Collapse all files

116
src/asyncio/async_can.cpp
View File

@@ -1,116 +0,0 @@
/*
* async_can.cpp
*
* Created on: Sep 10, 2020 13:23
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#include "wrp_sdk/asyncio/async_can.hpp"
#include <net/if.h>
#include <poll.h>
#include <string.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/can.h>
#include <iostream>
namespace westonrobot {
AsyncCAN::AsyncCAN(std::string can_port)
: AsyncListener(can_port), socketcan_stream_(io_context_) {}
bool AsyncCAN::SetupPort() {
try {
const size_t iface_name_size = strlen(port_.c_str()) + 1;
if (iface_name_size > IFNAMSIZ) return false;
can_fd_ = socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW);
if (can_fd_ < 0) return false;
struct ifreq ifr;
memset(&ifr, 0, sizeof(ifr));
memcpy(ifr.ifr_name, port_.c_str(), iface_name_size);
const int ioctl_result = ioctl(can_fd_, SIOCGIFINDEX, &ifr);
if (ioctl_result < 0) StopService();
struct sockaddr_can addr;
memset(&addr, 0, sizeof(addr));
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
const int bind_result =
bind(can_fd_, (struct sockaddr *)&addr, sizeof(addr));
if (bind_result < 0) StopService();
port_opened_ = true;
std::cout << "Start listening to port: " << port_ << std::endl;
} catch (std::system_error &e) {
std::cout << e.what();
return false;
}
// give some work to io_service to start async io chain
socketcan_stream_.assign(can_fd_);
asio::post(io_context_,
std::bind(&AsyncCAN::ReadFromPort, this, std::ref(rcv_frame_),
std::ref(socketcan_stream_)));
return true;
}
void AsyncCAN::StopService() {
// release port fd
const int close_result = ::close(can_fd_);
can_fd_ = -1;
// stop io thread
io_context_.stop();
if (io_thread_.joinable()) io_thread_.join();
io_context_.reset();
port_opened_ = false;
}
void AsyncCAN::DefaultReceiveCallback(can_frame *rx_frame) {
std::cout << std::hex << rx_frame->can_id << " ";
for (int i = 0; i < rx_frame->can_dlc; i++)
std::cout << std::hex << int(rx_frame->data[i]) << " ";
std::cout << std::dec << std::endl;
}
void AsyncCAN::ReadFromPort(struct can_frame &rec_frame,
asio::posix::basic_stream_descriptor<> &stream) {
auto sthis = shared_from_this();
stream.async_read_some(
asio::buffer(&rec_frame, sizeof(rec_frame)),
[sthis](asio::error_code error, size_t bytes_transferred) {
if (error) {
sthis->StopService();
return;
}
if (sthis->rcv_cb_ != nullptr)
sthis->rcv_cb_(&sthis->rcv_frame_);
else
sthis->DefaultReceiveCallback(&sthis->rcv_frame_);
sthis->ReadFromPort(std::ref(sthis->rcv_frame_),
std::ref(sthis->socketcan_stream_));
});
}
void AsyncCAN::SendFrame(const can_frame &frame) {
socketcan_stream_.async_write_some(
asio::buffer(&frame, sizeof(frame)),
[](asio::error_code error, size_t bytes_transferred) {
if (error) {
std::cerr << "Failed to send CAN frame" << std::endl;
}
// std::cout << "frame sent" << std::endl;
});
}
} // namespace westonrobot

150
src/asyncio/async_serial.cpp
View File

@@ -1,150 +0,0 @@
/*
* async_serial.cpp
*
* Created on: Sep 10, 2020 13:00
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#include "wrp_sdk/asyncio/async_serial.hpp"
#if defined(__linux__)
#include <linux/serial.h>
#endif
#include <cstring>
#include <iostream>
namespace westonrobot {
AsyncSerial::AsyncSerial(std::string port_name, uint32_t baud_rate)
: AsyncListener(port_name),
baud_rate_(baud_rate),
serial_port_(io_context_) {}
void AsyncSerial::SetBaudRate(unsigned baudrate) {
serial_port_.set_option(asio::serial_port_base::baud_rate(baudrate));
}
bool AsyncSerial::SetupPort() {
using SPB = asio::serial_port_base;
try {
serial_port_.open(port_);
// Set baudrate and 8N1 mode
serial_port_.set_option(SPB::baud_rate(baud_rate_));
serial_port_.set_option(SPB::character_size(8));
serial_port_.set_option(SPB::parity(SPB::parity::none));
serial_port_.set_option(SPB::stop_bits(SPB::stop_bits::one));
serial_port_.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_port_.native_handle();
struct serial_struct ser_info;
ioctl(fd, TIOCGSERIAL, &ser_info);
ser_info.flags |= ASYNC_LOW_LATENCY;
ioctl(fd, TIOCSSERIAL, &ser_info);
}
#endif
port_opened_ = true;
std::cout << "Start listening to port: " << port_ << "@" << baud_rate_
<< std::endl;
} catch (std::system_error &e) {
std::cout << e.what();
return false;
}
// give some work to io_service to start async io chain
asio::post(io_context_, std::bind(&AsyncSerial::ReadFromPort, this));
return true;
}
void AsyncSerial::StopService() {
if (!IsOpened()) return;
serial_port_.cancel();
serial_port_.close();
// stop io thread
io_context_.stop();
if (io_thread_.joinable()) io_thread_.join();
io_context_.reset();
port_opened_ = false;
}
void AsyncSerial::DefaultReceiveCallback(uint8_t *data, const size_t bufsize,
size_t len) {}
void AsyncSerial::ReadFromPort() {
auto sthis = shared_from_this();
serial_port_.async_read_some(
asio::buffer(rx_buf_),
[sthis](asio::error_code error, size_t bytes_transferred) {
if (error) {
sthis->StopService();
return;
}
if (sthis->rcv_cb_ != nullptr) {
sthis->rcv_cb_(sthis->rx_buf_.data(), sthis->rx_buf_.size(),
bytes_transferred);
} else {
sthis->DefaultReceiveCallback(
sthis->rx_buf_.data(), sthis->rx_buf_.size(), bytes_transferred);
}
sthis->ReadFromPort();
});
}
void AsyncSerial::WriteToPort(bool check_if_busy) {
// do nothing if an async tx has already been initiated
if (check_if_busy && tx_in_progress_) return;
std::lock_guard<std::recursive_mutex> lock(tx_mutex_);
if (tx_rbuf_.IsEmpty()) return;
auto sthis = shared_from_this();
tx_in_progress_ = true;
auto len = tx_rbuf_.Read(tx_buf_, tx_rbuf_.GetOccupiedSize());
serial_port_.async_write_some(
asio::buffer(tx_buf_, len),
[sthis](asio::error_code error, size_t bytes_transferred) {
if (error) {
sthis->StopService();
return;
}
std::lock_guard<std::recursive_mutex> lock(sthis->tx_mutex_);
if (sthis->tx_rbuf_.IsEmpty()) {
sthis->tx_in_progress_ = false;
return;
} else {
sthis->WriteToPort(false);
}
});
}
void AsyncSerial::SendBytes(const uint8_t *bytes, size_t length) {
if (!IsOpened()) {
std::cerr << "Failed to send, port closed" << std::endl;
return;
}
assert(length < rxtx_buffer_size);
std::lock_guard<std::recursive_mutex> lock(tx_mutex_);
if (tx_rbuf_.GetFreeSize() < length) {
throw std::length_error(
"AsyncSerial::SendBytes: tx buffer overflow, try to slow down sending "
"data");
}
tx_rbuf_.Write(bytes, length);
io_context_.post(
std::bind(&AsyncSerial::WriteToPort, shared_from_this(), true));
}
} // namespace westonrobot

172
src/platforms/hunter_base.cpp
View File

@@ -1,172 +0,0 @@
#include "wrp_sdk/platforms/hunter/hunter_base.hpp"
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <cstring>
#include <iostream>
#include <ratio>
#include <string>
#include <thread>
#include "stopwatch.h"
namespace westonrobot {
void HunterBase::SendRobotCmd() {
static uint8_t cmd_count = 0;
SendMotionCmd(cmd_count++);
}
void HunterBase::SendMotionCmd(uint8_t count) {
// motion control message
HunterMessage m_msg;
m_msg.type = HunterMotionCmdMsg;
if (can_connected_)
m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
else if (serial_connected_)
m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
motion_cmd_mutex_.lock();
m_msg.body.motion_cmd_msg.data.cmd.fault_clear_flag =
static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
m_msg.body.motion_cmd_msg.data.cmd.linear_velocity_cmd =
current_motion_cmd_.linear_velocity;
m_msg.body.motion_cmd_msg.data.cmd.angular_velocity_cmd =
current_motion_cmd_.angular_velocity;
motion_cmd_mutex_.unlock();
m_msg.body.motion_cmd_msg.data.cmd.reserved0 = 0;
m_msg.body.motion_cmd_msg.data.cmd.reserved1 = 0;
m_msg.body.motion_cmd_msg.data.cmd.count = count;
if (can_connected_)
m_msg.body.motion_cmd_msg.data.cmd.checksum = CalcHunterCANChecksum(
CAN_MSG_MOTION_CMD_ID, m_msg.body.motion_cmd_msg.data.raw, 8);
// send to can bus
if (can_connected_) {
can_frame m_frame;
EncodeHunterMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
}
}
HunterState HunterBase::GetHunterState() {
std::lock_guard<std::mutex> guard(hunter_state_mutex_);
return hunter_state_;
}
void HunterBase::SetMotionCommand(
double linear_vel, double steering_angle,
HunterMotionCmd::FaultClearFlag fault_clr_flag) {
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_) StartCmdThread();
if (linear_vel < HunterMotionCmd::min_linear_velocity)
linear_vel = HunterMotionCmd::min_linear_velocity;
if (linear_vel > HunterMotionCmd::max_linear_velocity)
linear_vel = HunterMotionCmd::max_linear_velocity;
if (steering_angle < HunterMotionCmd::min_steering_angle)
steering_angle = HunterMotionCmd::min_steering_angle;
if (steering_angle > HunterMotionCmd::max_steering_angle)
steering_angle = HunterMotionCmd::max_steering_angle;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(
linear_vel / HunterMotionCmd::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(
steering_angle / HunterMotionCmd::max_steering_angle * 100.0);
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
FeedCmdTimeoutWatchdog();
}
void HunterBase::ParseCANFrame(can_frame *rx_frame) {
// validate checksum, discard frame if fails
if (!rx_frame->data[7] == CalcHunterCANChecksum(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
HunterMessage status_msg;
DecodeHunterMsgFromCAN(rx_frame, &status_msg);
NewStatusMsgReceivedCallback(status_msg);
}
void HunterBase::NewStatusMsgReceivedCallback(const HunterMessage &msg) {
// std::cout << "new status msg received" << std::endl;
std::lock_guard<std::mutex> guard(hunter_state_mutex_);
UpdateHunterState(msg, hunter_state_);
}
void HunterBase::UpdateHunterState(const HunterMessage &status_msg,
HunterState &state) {
switch (status_msg.type) {
case HunterMotionStatusMsg: {
// std::cout << "motion control feedback received" << std::endl;
const MotionStatusMessage &msg = status_msg.body.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.steering_angle =
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 HunterSystemStatusMsg: {
// std::cout << "system status feedback received" << std::endl;
const SystemStatusMessage &msg = status_msg.body.system_status_msg;
state.control_mode = msg.data.status.control_mode;
state.base_state = msg.data.status.base_state;
state.battery_voltage =
(static_cast<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 HunterMotorDriverStatusMsg: {
// std::cout << "motor 1 driver feedback received" << std::endl;
const MotorDriverStatusMessage &msg =
status_msg.body.motor_driver_status_msg;
for (int i = 0; i < HunterState::motor_num; ++i) {
state.motor_states[status_msg.body.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.body.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.body.motor_driver_status_msg.motor_id]
.temperature = msg.data.status.temperature;
}
break;
}
case HunterConfigStatusMsg: {
const ConfigStatusMessage &msg = status_msg.body.config_status_msg;
state.set_zero_steering = msg.data.status.set_zero_steering;
break;
}
default:
break;
}
}
} // namespace westonrobot

141
src/platforms/hunter_can_parser.c
View File

@@ -1,141 +0,0 @@
/*
* hunter_can_parser.c
*
* Created on: Jan 02, 2020 12:40
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "wrp_sdk/platforms/hunter/hunter_can_parser.h"
#include "string.h"
static void EncodeHunterMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame);
bool DecodeHunterMsgFromCAN(const struct can_frame *rx_frame, HunterMessage *msg)
{
msg->type = HunterMsgNone;
switch (rx_frame->can_id)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_STATUS_ID:
{
msg->type = HunterMotionStatusMsg;
memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_SYSTEM_STATUS_STATUS_ID:
{
msg->type = HunterSystemStatusMsg;
memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_CONFIG_STATUS_ID:
{
msg->type = HunterSystemStatusMsg;
memcpy(msg->body.config_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR1_DRIVER_STATUS_ID:
{
msg->type = HunterMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = HUNTER_MOTOR1_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR2_DRIVER_STATUS_ID:
{
msg->type = HunterMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = HUNTER_MOTOR2_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR3_DRIVER_STATUS_ID:
{
msg->type = HunterMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = HUNTER_MOTOR3_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_CMD_ID:
{
msg->type = HunterMotionCmdMsg;
memcpy(msg->body.motion_cmd_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_CONFIG_CMD_ID:
{
msg->type = HunterConfigCmdMsg;
memcpy(msg->body.config_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
default:
break;
}
return true;
}
void EncodeHunterMsgToCAN(const HunterMessage *msg, struct can_frame *tx_frame)
{
switch (msg->type)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case HunterMotionStatusMsg:
{
tx_frame->can_id = CAN_MSG_MOTION_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case HunterSystemStatusMsg:
{
tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case HunterMotorDriverStatusMsg:
{
if (msg->body.motor_driver_status_msg.motor_id == HUNTER_MOTOR1_ID)
tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == HUNTER_MOTOR2_ID)
tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == HUNTER_MOTOR3_ID)
tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
// else if (msg->body.motor_driver_status_msg.motor_id == HUNTER_MOTOR4_ID)
// tx_frame->can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case HunterMotionCmdMsg:
{
EncodeHunterMotionControlMsgToCAN(&(msg->body.motion_cmd_msg), tx_frame);
break;
}
default:
break;
}
tx_frame->data[7] = CalcHunterCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeHunterMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_MOTION_CMD_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcHunterCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
uint8_t CalcHunterCANChecksum(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;
}

96
src/platforms/mobile_base.cpp
View File

@@ -1,96 +0,0 @@
/*
* mobile_base.cpp
*
* Created on: Jun 17, 2020 11:26
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#include "wrp_sdk/platforms/common/mobile_base.hpp"
#include <cstring>
#include <iostream>
#include <algorithm>
#include "stopwatch.h"
namespace westonrobot {
MobileBase::~MobileBase() {
// release resource if occupied
Disconnect();
// joint cmd thread
if (cmd_thread_.joinable()) cmd_thread_.join();
}
void MobileBase::Connect(std::string dev_name, int32_t baud_rate) {
if (baud_rate == 0) {
ConfigureCAN(dev_name);
} else {
ConfigureSerial(dev_name, baud_rate);
if (!serial_connected_)
std::cerr << "ERROR: Failed to connect to serial port" << std::endl;
}
}
void MobileBase::Disconnect() {
if (can_connected_) can_if_->StopService();
if (serial_connected_ && serial_if_->IsOpened()) {
serial_if_->StopService();
}
}
void MobileBase::ConfigureCAN(const std::string &can_if_name) {
can_if_ = std::make_shared<AsyncCAN>(can_if_name);
can_if_->SetReceiveCallback(
std::bind(&MobileBase::ParseCANFrame, this, std::placeholders::_1));
can_if_->StartListening();
can_connected_ = true;
}
void MobileBase::ConfigureSerial(const std::string uart_name,
int32_t baud_rate) {
serial_if_ = std::make_shared<AsyncSerial>(uart_name, baud_rate);
serial_if_->SetReceiveCallback(
std::bind(&MobileBase::ParseUARTBuffer, this, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3));
serial_if_->StartListening();
if (serial_if_->IsOpened()) serial_connected_ = true;
}
void MobileBase::StartCmdThread() {
cmd_thread_ = std::thread(
std::bind(&MobileBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true;
}
void MobileBase::ControlLoop(int32_t period_ms) {
StopWatch ctrl_sw;
bool print_loop_freq = false;
uint32_t timeout_iter_num;
if (enable_timeout_) {
if (timeout_ms_ < period_ms) timeout_ms_ = period_ms;
timeout_iter_num = static_cast<uint32_t>(timeout_ms_ / period_ms);
std::cout << "Timeout iteration number: " << timeout_iter_num << std::endl;
}
while (true) {
ctrl_sw.tic();
if (enable_timeout_) {
if (watchdog_counter_ < timeout_iter_num) {
SendRobotCmd();
++watchdog_counter_;
} else {
std::cout << "Warning: cmd timeout, old cmd not sent to robot" << std::endl;
}
} else {
SendRobotCmd();
}
ctrl_sw.sleep_until_ms(period_ms);
if (print_loop_freq)
std::cout << "control loop frequency: " << 1.0 / ctrl_sw.toc()
<< std::endl;
}
}
} // namespace westonrobot

285
src/platforms/scout_base.cpp
View File

@@ -1,285 +0,0 @@
#include "wrp_sdk/platforms/scout/scout_base.hpp"
#include <string>
#include <cstring>
#include <iostream>
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <ratio>
#include <thread>
#include "stopwatch.h"
namespace westonrobot {
void ScoutBase::SendRobotCmd() {
static uint8_t cmd_count = 0;
static uint8_t light_cmd_count = 0;
SendMotionCmd(cmd_count++);
if (light_ctrl_requested_) SendLightCmd(light_cmd_count++);
}
void ScoutBase::SendMotionCmd(uint8_t count) {
// motion control message
ScoutMessage m_msg;
m_msg.type = ScoutMotionControlMsg;
if (can_connected_)
m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
else if (serial_connected_)
m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
motion_cmd_mutex_.lock();
m_msg.body.motion_control_msg.data.cmd.fault_clear_flag =
static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd =
current_motion_cmd_.linear_velocity;
m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd =
current_motion_cmd_.angular_velocity;
motion_cmd_mutex_.unlock();
m_msg.body.motion_control_msg.data.cmd.reserved0 = 0;
m_msg.body.motion_control_msg.data.cmd.reserved1 = 0;
m_msg.body.motion_control_msg.data.cmd.count = count;
if (can_connected_)
m_msg.body.motion_control_msg.data.cmd.checksum =
CalcScoutCANChecksum(CAN_MSG_MOTION_CONTROL_CMD_ID,
m_msg.body.motion_control_msg.data.raw, 8);
// serial_connected_: checksum will be calculated later when packed into a
// complete serial frame
if (can_connected_) {
// send to can bus
can_frame m_frame;
EncodeScoutMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame);
// std::cout << "CAN cmd sent" << std::endl;
} else {
// send to serial port
EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
serial_if_->SendBytes(tx_buffer_, tx_cmd_len_);
// std::cout << "serial cmd sent" << std::endl;
}
}
void ScoutBase::SendLightCmd(uint8_t count) {
ScoutMessage l_msg;
l_msg.type = ScoutLightControlMsg;
light_cmd_mutex_.lock();
if (light_ctrl_enabled_) {
l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL;
l_msg.body.light_control_msg.data.cmd.front_light_mode =
static_cast<uint8_t>(current_light_cmd_.front_mode);
l_msg.body.light_control_msg.data.cmd.front_light_custom =
current_light_cmd_.front_custom_value;
l_msg.body.light_control_msg.data.cmd.rear_light_mode =
static_cast<uint8_t>(current_light_cmd_.rear_mode);
l_msg.body.light_control_msg.data.cmd.rear_light_custom =
current_light_cmd_.rear_custom_value;
// std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " <<
// l_msg.data.cmd.front_light_custom << " , "
// << l_msg.data.cmd.rear_light_mode << " , " <<
// l_msg.data.cmd.rear_light_custom << std::endl;
// std::cout << "light cmd generated" << std::endl;
} else {
l_msg.body.light_control_msg.data.cmd.light_ctrl_enable =
LIGHT_DISABLE_CTRL;
l_msg.body.light_control_msg.data.cmd.front_light_mode =
LIGHT_MODE_CONST_OFF;
l_msg.body.light_control_msg.data.cmd.front_light_custom = 0;
l_msg.body.light_control_msg.data.cmd.rear_light_mode =
LIGHT_MODE_CONST_OFF;
l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0;
}
light_ctrl_requested_ = false;
light_cmd_mutex_.unlock();
l_msg.body.light_control_msg.data.cmd.reserved0 = 0;
l_msg.body.light_control_msg.data.cmd.count = count;
if (can_connected_)
l_msg.body.light_control_msg.data.cmd.checksum = CalcScoutCANChecksum(
CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8);
// serial_connected_: checksum will be calculated later when packed into a
// complete serial frame
if (can_connected_) {
// send to can bus
can_frame l_frame;
EncodeScoutMsgToCAN(&l_msg, &l_frame);
can_if_->SendFrame(l_frame);
} else {
// send to serial port
EncodeScoutMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_);
serial_if_->SendBytes(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;
}
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;
FeedCmdTimeoutWatchdog();
}
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
ScoutMessage status_msg;
DecodeScoutMsgFromCAN(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;
// serial_parser_.PrintStatistics();
// serial_parser_.ParseBuffer(buf, bytes_received);
ScoutMessage status_msg;
for (int i = 0; i < bytes_received; ++i) {
if (DecodeScoutMsgFromUART(buf[i], &status_msg))
NewStatusMsgReceivedCallback(status_msg);
}
}
void ScoutBase::NewStatusMsgReceivedCallback(const ScoutMessage &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 ScoutMessage &status_msg,
ScoutState &state) {
switch (status_msg.type) {
case ScoutMotionStatusMsg: {
// std::cout << "motion control feedback received" << std::endl;
const MotionStatusMessage &msg = status_msg.body.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.body.light_status_msg;
if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL)
state.light_control_enabled = false;
else
state.light_control_enabled = true;
state.front_light_state.mode = msg.data.status.front_light_mode;
state.front_light_state.custom_value = msg.data.status.front_light_custom;
state.rear_light_state.mode = msg.data.status.rear_light_mode;
state.rear_light_state.custom_value = msg.data.status.rear_light_custom;
break;
}
case ScoutSystemStatusMsg: {
// std::cout << "system status feedback received" << std::endl;
const SystemStatusMessage &msg = status_msg.body.system_status_msg;
state.control_mode = msg.data.status.control_mode;
state.base_state = msg.data.status.base_state;
state.battery_voltage =
(static_cast<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.body.motor_driver_status_msg;
for (int i = 0; i < ScoutState::motor_num; ++i) {
state.motor_states[status_msg.body.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.body.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.body.motor_driver_status_msg.motor_id]
.temperature = msg.data.status.temperature;
}
break;
}
}
}
} // namespace westonrobot

178
src/platforms/scout_can_parser.c
View File

@@ -1,178 +0,0 @@
/*
* scout_can_parser.c
*
* Created on: Aug 31, 2019 04:25
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "wrp_sdk/platforms/scout/scout_can_parser.h"
#include "string.h"
static void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame);
static void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame);
bool DecodeScoutMsgFromCAN(const struct can_frame *rx_frame, ScoutMessage *msg)
{
msg->type = ScoutMsgNone;
switch (rx_frame->can_id)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_CONTROL_STATUS_ID:
{
msg->type = ScoutMotionStatusMsg;
// msg->motion_status_msg.id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_LIGHT_CONTROL_STATUS_ID:
{
msg->type = ScoutLightStatusMsg;
// msg->light_status_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
memcpy(msg->body.light_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_SYSTEM_STATUS_STATUS_ID:
{
msg->type = ScoutSystemStatusMsg;
// msg->system_status_msg.id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR1_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR2_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR3_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR4_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
// msg->motor_driver_status_msg.id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_CONTROL_CMD_ID:
{
msg->type = ScoutMotionControlMsg;
// msg->motion_control_msg.id = CAN_MSG_MOTION_CONTROL_CMD_ID;
memcpy(msg->body.motion_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_LIGHT_CONTROL_CMD_ID:
{
msg->type = ScoutLightControlMsg;
// msg->light_control_msg.id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
memcpy(msg->body.light_control_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
default:
break;
}
return true;
}
void EncodeScoutMsgToCAN(const ScoutMessage *msg, struct can_frame *tx_frame)
{
switch (msg->type)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case ScoutMotionStatusMsg:
{
tx_frame->can_id = CAN_MSG_MOTION_CONTROL_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutLightStatusMsg:
{
tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.light_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutSystemStatusMsg:
{
tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutMotorDriverStatusMsg:
{
if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID)
tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID)
tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID)
tx_frame->can_id = CAN_MSG_MOTOR3_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID)
tx_frame->can_id = CAN_MSG_MOTOR4_DRIVER_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case ScoutMotionControlMsg:
{
EncodeScoutMotionControlMsgToCAN(&(msg->body.motion_control_msg), tx_frame);
break;
}
case ScoutLightControlMsg:
{
EncodeScoutLightControlMsgToCAN(&(msg->body.light_control_msg), tx_frame);
break;
}
default:
break;
}
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeScoutMotionControlMsgToCAN(const MotionControlMessage *msg, struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_MOTION_CONTROL_CMD_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeScoutLightControlMsgToCAN(const LightControlMessage *msg, struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_LIGHT_CONTROL_CMD_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcScoutCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
uint8_t CalcScoutCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
{
uint8_t checksum = 0x00;
checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc;
for (int i = 0; i < (dlc - 1); ++i)
checksum += data[i];
return checksum;
}

629
src/platforms/scout_uart_parser.c
View File

@@ -1,629 +0,0 @@
/*
* scout_uart_parser.c
*
* Created on: Aug 14, 2019 12:02
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "wrp_sdk/platforms/scout/scout_uart_parser.h"
// #define USE_XOR_CHECKSUM
// #define PRINT_CPP_DEBUG_INFO
// #define PRINT_JLINK_DEBUG_INFO
#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)
// frame buffer
static struct
{
uint8_t frame_id;
uint8_t frame_type;
uint8_t frame_len;
uint8_t frame_cnt;
uint8_t frame_checksum;
uint8_t internal_checksum;
uint8_t payload_buffer[PAYLOAD_BUFFER_SIZE];
size_t payload_data_pos;
} uart_parsing_data;
// statisctics
typedef struct
{
uint32_t frame_parsed;
uint32_t frame_with_wrong_checksum;
} UARTParsingStats;
static UARTParsingStats uart_parsing_stats = {.frame_parsed = true, .frame_with_wrong_checksum = 123};
// internal functions
static bool ParseChar(uint8_t c, ScoutMessage *msg);
static uint8_t CalcBufferedFrameChecksum();
static bool ConstructStatusMessage(ScoutMessage *msg);
static bool ConstructControlMessage(ScoutMessage *msg);
static void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len);
static void EncodeLightControlMsgToUART(const LightControlMessage *msg, uint8_t *buf, uint8_t *len);
void EncodeScoutMsgToUART(const ScoutMessage *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->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->body.motion_status_msg.data.status.linear_velocity.high_byte;
buf[6] = msg->body.motion_status_msg.data.status.linear_velocity.low_byte;
buf[7] = msg->body.motion_status_msg.data.status.angular_velocity.high_byte;
buf[8] = msg->body.motion_status_msg.data.status.angular_velocity.low_byte;
buf[9] = 0;
buf[10] = 0;
buf[11] = msg->body.motion_status_msg.data.status.count;
break;
}
case ScoutLightStatusMsg:
{
buf[4] = UART_FRAME_LIGHT_STATUS_ID;
buf[5] = msg->body.light_status_msg.data.status.light_ctrl_enable;
buf[6] = msg->body.light_status_msg.data.status.front_light_mode;
buf[7] = msg->body.light_status_msg.data.status.front_light_custom;
buf[8] = msg->body.light_status_msg.data.status.rear_light_mode;
buf[9] = msg->body.light_status_msg.data.status.rear_light_custom;
buf[10] = 0;
buf[11] = msg->body.light_status_msg.data.status.count;
break;
}
case ScoutSystemStatusMsg:
{
buf[4] = UART_FRAME_SYSTEM_STATUS_ID;
buf[5] = msg->body.system_status_msg.data.status.base_state;
buf[6] = msg->body.system_status_msg.data.status.control_mode;
buf[7] = msg->body.system_status_msg.data.status.battery_voltage.high_byte;
buf[8] = msg->body.system_status_msg.data.status.battery_voltage.low_byte;
buf[9] = msg->body.system_status_msg.data.status.fault_code.high_byte;
buf[10] = msg->body.system_status_msg.data.status.fault_code.low_byte;
buf[11] = msg->body.system_status_msg.data.status.count;
break;
}
case ScoutMotorDriverStatusMsg:
{
if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR1_ID)
buf[4] = UART_FRAME_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR2_ID)
buf[4] = UART_FRAME_MOTOR2_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR3_ID)
buf[4] = UART_FRAME_MOTOR3_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == SCOUT_MOTOR4_ID)
buf[4] = UART_FRAME_MOTOR4_DRIVER_STATUS_ID;
buf[5] = msg->body.motor_driver_status_msg.data.status.current.high_byte;
buf[6] = msg->body.motor_driver_status_msg.data.status.current.low_byte;
buf[7] = msg->body.motor_driver_status_msg.data.status.rpm.high_byte;
buf[8] = msg->body.motor_driver_status_msg.data.status.rpm.low_byte;
buf[9] = msg->body.motor_driver_status_msg.data.status.temperature;
buf[10] = 0;
buf[11] = msg->body.motor_driver_status_msg.data.status.count;
break;
}
case ScoutMotionControlMsg:
{
EncodeMotionControlMsgToUART(&(msg->body.motion_control_msg), buf, len);
break;
}
case ScoutLightControlMsg:
{
EncodeLightControlMsgToUART(&(msg->body.light_control_msg), buf, len);
break;
}
default:
break;
}
buf[12] = CalcScoutUARTChecksum(buf, buf[2] + FRAME_SOF_LEN);
// length: SOF + Frame + Checksum
*len = buf[2] + FRAME_SOF_LEN + 1;
}
bool DecodeScoutMsgFromUART(uint8_t c, ScoutMessage *msg)
{
static ScoutMessage decoded_msg;
bool result = ParseChar(c, &decoded_msg);
if (result)
*msg = decoded_msg;
return result;
}
void EncodeMotionControlMsgToUART(const MotionControlMessage *msg, uint8_t *buf, uint8_t *len)
{
// SOF
buf[0] = 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 ParseChar(uint8_t c, ScoutMessage *msg)
{
static ScoutSerialDecodeState decode_state = WAIT_FOR_SOF1;
bool new_frame_parsed = false;
switch (decode_state)
{
case WAIT_FOR_SOF1:
{
if (c == FRAME_SOF1)
{
uart_parsing_data.frame_id = FRAME_NONE_ID;
uart_parsing_data.frame_type = 0;
uart_parsing_data.frame_len = 0;
uart_parsing_data.frame_cnt = 0;
uart_parsing_data.frame_checksum = 0;
uart_parsing_data.internal_checksum = 0;
uart_parsing_data.payload_data_pos = 0;
memset(uart_parsing_data.payload_buffer, 0, 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:
{
uart_parsing_data.frame_len = c;
decode_state = WAIT_FOR_FRAME_TYPE;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "frame len: " << std::hex << static_cast<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:
{
uart_parsing_data.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:
{
uart_parsing_data.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:
{
uart_parsing_data.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:
{
uart_parsing_data.payload_buffer[uart_parsing_data.payload_data_pos++] = c;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "1 byte added: " << std::hex << static_cast<int>(c) << std::dec << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkRTTPrintf(0, "1 byte added: %d\n", c);
#endif
if (uart_parsing_data.payload_data_pos == (uart_parsing_data.frame_len - FRAME_FIXED_FIELD_LEN))
decode_state = WAIT_FOR_FRAME_COUNT;
break;
}
case WAIT_FOR_FRAME_COUNT:
{
uart_parsing_data.frame_cnt = c;
decode_state = WAIT_FOR_CHECKSUM;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "frame count: " << std::hex << static_cast<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:
{
uart_parsing_data.frame_checksum = c;
uart_parsing_data.internal_checksum = CalcBufferedFrameChecksum();
new_frame_parsed = true;
decode_state = WAIT_FOR_SOF1;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "--- frame checksum: " << std::hex << static_cast<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 (uart_parsing_data.frame_checksum == uart_parsing_data.internal_checksum)
{
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "checksum correct" << std::endl;
#elif (defined(PRINT_JLINK_DEBUG_INFO))
JLinkWriteString(0, "checksum correct\n");
#endif
if (uart_parsing_data.frame_type == FRAME_TYPE_STATUS)
ConstructStatusMessage(msg);
else if (uart_parsing_data.frame_type == FRAME_TYPE_CONTROL)
ConstructControlMessage(msg);
++uart_parsing_stats.frame_parsed;
}
else
{
++uart_parsing_stats.frame_with_wrong_checksum;
#ifdef PRINT_CPP_DEBUG_INFO
std::cout << "checksum is NOT correct" << std::endl;
std::cout << std::hex << static_cast<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(ScoutMessage *msg)
{
if (msg == NULL)
return false;
switch (uart_parsing_data.frame_id)
{
case UART_FRAME_MOTION_CONTROL_ID:
{
msg->type = ScoutMotionControlMsg;
msg->body.motion_control_msg.data.cmd.control_mode = uart_parsing_data.payload_buffer[0];
msg->body.motion_control_msg.data.cmd.fault_clear_flag = uart_parsing_data.payload_buffer[1];
msg->body.motion_control_msg.data.cmd.linear_velocity_cmd = uart_parsing_data.payload_buffer[2];
msg->body.motion_control_msg.data.cmd.angular_velocity_cmd = uart_parsing_data.payload_buffer[3];
msg->body.motion_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[4];
msg->body.motion_control_msg.data.cmd.reserved1 = uart_parsing_data.payload_buffer[5];
msg->body.motion_control_msg.data.cmd.count = uart_parsing_data.frame_cnt;
msg->body.motion_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_LIGHT_CONTROL_ID:
{
msg->type = ScoutLightControlMsg;
msg->body.light_control_msg.data.cmd.light_ctrl_enable = uart_parsing_data.payload_buffer[0];
msg->body.light_control_msg.data.cmd.front_light_mode = uart_parsing_data.payload_buffer[1];
msg->body.light_control_msg.data.cmd.front_light_custom = uart_parsing_data.payload_buffer[2];
msg->body.light_control_msg.data.cmd.rear_light_mode = uart_parsing_data.payload_buffer[3];
msg->body.light_control_msg.data.cmd.rear_light_custom = uart_parsing_data.payload_buffer[4];
msg->body.light_control_msg.data.cmd.reserved0 = uart_parsing_data.payload_buffer[5];
msg->body.light_control_msg.data.cmd.count = uart_parsing_data.frame_cnt;
msg->body.light_control_msg.data.cmd.checksum = uart_parsing_data.frame_checksum;
break;
}
}
return true;
}
bool ConstructStatusMessage(ScoutMessage *msg)
{
if (msg == NULL)
return false;
switch (uart_parsing_data.frame_id)
{
case UART_FRAME_SYSTEM_STATUS_ID:
{
msg->type = ScoutSystemStatusMsg;
msg->body.system_status_msg.data.status.base_state = uart_parsing_data.payload_buffer[0];
msg->body.system_status_msg.data.status.control_mode = uart_parsing_data.payload_buffer[1];
msg->body.system_status_msg.data.status.battery_voltage.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.system_status_msg.data.status.battery_voltage.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.system_status_msg.data.status.fault_code.high_byte = uart_parsing_data.payload_buffer[4];
msg->body.system_status_msg.data.status.fault_code.low_byte = uart_parsing_data.payload_buffer[5];
msg->body.system_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.system_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTION_STATUS_ID:
{
msg->type = ScoutMotionStatusMsg;
msg->body.motion_status_msg.data.status.linear_velocity.high_byte = uart_parsing_data.payload_buffer[0];
msg->body.motion_status_msg.data.status.linear_velocity.low_byte = uart_parsing_data.payload_buffer[1];
msg->body.motion_status_msg.data.status.angular_velocity.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.motion_status_msg.data.status.angular_velocity.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.motion_status_msg.data.status.reserved0 = 0x00;
msg->body.motion_status_msg.data.status.reserved0 = 0x00;
msg->body.motion_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.motion_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTOR1_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR1_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTOR2_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR2_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTOR3_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR3_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_MOTOR4_DRIVER_STATUS_ID:
{
msg->type = ScoutMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = SCOUT_MOTOR4_ID;
msg->body.motor_driver_status_msg.data.status.current.high_byte = uart_parsing_data.payload_buffer[0];
msg->body.motor_driver_status_msg.data.status.current.low_byte = uart_parsing_data.payload_buffer[1];
msg->body.motor_driver_status_msg.data.status.rpm.high_byte = uart_parsing_data.payload_buffer[2];
msg->body.motor_driver_status_msg.data.status.rpm.low_byte = uart_parsing_data.payload_buffer[3];
msg->body.motor_driver_status_msg.data.status.temperature = uart_parsing_data.payload_buffer[4];
msg->body.motor_driver_status_msg.data.status.reserved0 = 0x00;
msg->body.motor_driver_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.motor_driver_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
case UART_FRAME_LIGHT_STATUS_ID:
{
msg->type = ScoutLightStatusMsg;
msg->body.light_status_msg.data.status.light_ctrl_enable = uart_parsing_data.payload_buffer[0];
msg->body.light_status_msg.data.status.front_light_mode = uart_parsing_data.payload_buffer[1];
msg->body.light_status_msg.data.status.front_light_custom = uart_parsing_data.payload_buffer[2];
msg->body.light_status_msg.data.status.rear_light_mode = uart_parsing_data.payload_buffer[3];
msg->body.light_status_msg.data.status.rear_light_custom = uart_parsing_data.payload_buffer[4];
msg->body.light_status_msg.data.status.reserved0 = 0x00;
msg->body.light_status_msg.data.status.count = uart_parsing_data.frame_cnt;
msg->body.light_status_msg.data.status.checksum = uart_parsing_data.frame_checksum;
break;
}
}
return true;
}
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 ^= uart_parsing_data.frame_len;
checksum ^= uart_parsing_data.frame_type;
checksum ^= uart_parsing_data.frame_id;
for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i)
checksum ^= uart_parsing_data.payload_buffer[i];
checksum ^= uart_parsing_data.frame_cnt;
#else
checksum += FRAME_SOF1;
checksum += FRAME_SOF2;
checksum += uart_parsing_data.frame_len;
checksum += uart_parsing_data.frame_type;
checksum += uart_parsing_data.frame_id;
for (size_t i = 0; i < uart_parsing_data.payload_data_pos; ++i)
checksum += uart_parsing_data.payload_buffer[i];
checksum += uart_parsing_data.frame_cnt;
#endif
return checksum;
}

338
src/platforms/tracer_base.cpp
View File

@@ -1,338 +0,0 @@
#include "tracer_base/tracer_base.hpp"
#include <string>
#include <cstring>
#include <iostream>
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <ratio>
#include <thread>
#include "stopwatch/stopwatch.h"
namespace westonrobot
{
TracerBase::~TracerBase()
{
if (serial_connected_)
serial_if_->close();
if (cmd_thread_.joinable())
cmd_thread_.join();
}
void TracerBase::Connect(std::string dev_name)
{
// if (baud_rate == 0) {
ConfigureCANBus(dev_name);
// } else {
// ConfigureSerial(dev_name, baud_rate);
// if (!serial_connected_)
// std::cerr << "ERROR: Failed to connect to serial port" << std::endl;
// }
}
void TracerBase::Disconnect()
{
if (serial_connected_)
{
if (serial_if_->is_open())
serial_if_->close();
}
}
void TracerBase::ConfigureCANBus(const std::string &can_if_name)
{
can_if_ = std::make_shared<AsyncCAN>(can_if_name);
can_if_->set_receive_callback(std::bind(&TracerBase::ParseCANFrame, this, std::placeholders::_1));
can_connected_ = true;
}
void TracerBase::ConfigureSerial(const std::string uart_name, int32_t baud_rate)
{
serial_if_ = std::make_shared<AsyncSerial>(uart_name, baud_rate);
serial_if_->open();
if (serial_if_->is_open())
serial_connected_ = true;
serial_if_->set_receive_callback(std::bind(&TracerBase::ParseUARTBuffer, this,
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3));
}
void TracerBase::StartCmdThread()
{
current_motion_cmd_.linear_velocity = 0;
current_motion_cmd_.angular_velocity = 0;
current_motion_cmd_.fault_clear_flag = TracerMotionCmd::FaultClearFlag::NO_FAULT;
cmd_thread_ = std::thread(std::bind(&TracerBase::ControlLoop, this, cmd_thread_period_ms_));
cmd_thread_started_ = true;
}
void TracerBase::SendMotionCmd(uint8_t count)
{
// motion control message
TracerMessage m_msg;
m_msg.type = TracerMotionCmdMsg;
if (can_connected_)
m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_CAN;
else if (serial_connected_)
m_msg.body.motion_cmd_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
motion_cmd_mutex_.lock();
m_msg.body.motion_cmd_msg.data.cmd.fault_clear_flag = static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
m_msg.body.motion_cmd_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity;
m_msg.body.motion_cmd_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity;
motion_cmd_mutex_.unlock();
m_msg.body.motion_cmd_msg.data.cmd.reserved0 = 0;
m_msg.body.motion_cmd_msg.data.cmd.reserved1 = 0;
m_msg.body.motion_cmd_msg.data.cmd.count = count;
if (can_connected_)
m_msg.body.motion_cmd_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_MOTION_CMD_ID, m_msg.body.motion_cmd_msg.data.raw, 8);
// serial_connected_: checksum will be calculated later when packed into a complete serial frame
if (can_connected_)
{
// send to can bus
can_frame m_frame;
EncodeTracerMsgToCAN(&m_msg, &m_frame);
can_if_->send_frame(m_frame);
}
else
{
// TODO
// send to serial port
// EncodeTracerMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
// serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
}
}
void TracerBase::SendLightCmd(uint8_t count)
{
TracerMessage l_msg;
l_msg.type = TracerLightControlMsg;
light_cmd_mutex_.lock();
if (light_ctrl_enabled_)
{
l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_ENABLE_CTRL;
l_msg.body.light_control_msg.data.cmd.front_light_mode = static_cast<uint8_t>(current_light_cmd_.front_mode);
l_msg.body.light_control_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value;
l_msg.body.light_control_msg.data.cmd.rear_light_mode = static_cast<uint8_t>(current_light_cmd_.rear_mode);
l_msg.body.light_control_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value;
// std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " << l_msg.data.cmd.front_light_custom << " , "
// << l_msg.data.cmd.rear_light_mode << " , " << l_msg.data.cmd.rear_light_custom << std::endl;
// std::cout << "light cmd generated" << std::endl;
}
else
{
l_msg.body.light_control_msg.data.cmd.light_ctrl_enable = LIGHT_DISABLE_CTRL;
l_msg.body.light_control_msg.data.cmd.front_light_mode = LIGHT_MODE_CONST_OFF;
l_msg.body.light_control_msg.data.cmd.front_light_custom = 0;
l_msg.body.light_control_msg.data.cmd.rear_light_mode = LIGHT_MODE_CONST_OFF;
l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0;
}
light_ctrl_requested_ = false;
light_cmd_mutex_.unlock();
l_msg.body.light_control_msg.data.cmd.reserved0 = 0;
l_msg.body.light_control_msg.data.cmd.count = count;
if (can_connected_)
l_msg.body.light_control_msg.data.cmd.checksum = CalcTracerCANChecksum(CAN_MSG_LIGHT_CONTROL_CMD_ID, l_msg.body.light_control_msg.data.raw, 8);
// serial_connected_: checksum will be calculated later when packed into a complete serial frame
if (can_connected_)
{
// send to can bus
can_frame l_frame;
EncodeTracerMsgToCAN(&l_msg, &l_frame);
can_if_->send_frame(l_frame);
}
// else
// {
// // send to serial port
// EncodeTracerMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_);
// serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
// }
// std::cout << "cmd: " << static_cast<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 TracerBase::ControlLoop(int32_t period_ms)
{
StopWatch ctrl_sw;
uint8_t cmd_count = 0;
uint8_t light_cmd_count = 0;
while (true)
{
ctrl_sw.tic();
// motion control message
SendMotionCmd(cmd_count++);
// check if there is request for light control
if (light_ctrl_requested_)
SendLightCmd(light_cmd_count++);
ctrl_sw.sleep_until_ms(period_ms);
// std::cout << "control loop update frequency: " << 1.0 / ctrl_sw.toc() << std::endl;
}
}
TracerState TracerBase::GetTracerState()
{
std::lock_guard<std::mutex> guard(tracer_state_mutex_);
return tracer_state_;
}
void TracerBase::SetMotionCommand(double linear_vel, double angular_vel, TracerMotionCmd::FaultClearFlag fault_clr_flag)
{
// make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_)
StartCmdThread();
if (linear_vel < TracerMotionCmd::min_linear_velocity)
linear_vel = TracerMotionCmd::min_linear_velocity;
if (linear_vel > TracerMotionCmd::max_linear_velocity)
linear_vel = TracerMotionCmd::max_linear_velocity;
if (angular_vel < TracerMotionCmd::min_angular_velocity)
angular_vel = TracerMotionCmd::min_angular_velocity;
if (angular_vel > TracerMotionCmd::max_angular_velocity)
angular_vel = TracerMotionCmd::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_);
current_motion_cmd_.linear_velocity = static_cast<int8_t>(linear_vel / TracerMotionCmd::max_linear_velocity * 100.0);
current_motion_cmd_.angular_velocity = static_cast<int8_t>(angular_vel / TracerMotionCmd::max_angular_velocity * 100.0);
current_motion_cmd_.fault_clear_flag = fault_clr_flag;
}
void TracerBase::SetLightCommand(TracerLightCmd cmd)
{
if (!cmd_thread_started_)
StartCmdThread();
std::lock_guard<std::mutex> guard(light_cmd_mutex_);
current_light_cmd_ = cmd;
light_ctrl_enabled_ = true;
light_ctrl_requested_ = true;
}
void TracerBase::DisableLightCmdControl()
{
std::lock_guard<std::mutex> guard(light_cmd_mutex_);
light_ctrl_enabled_ = false;
light_ctrl_requested_ = true;
}
void TracerBase::ParseCANFrame(can_frame *rx_frame)
{
// validate checksum, discard frame if fails
if (!rx_frame->data[7] == CalcTracerCANChecksum(rx_frame->can_id, rx_frame->data, rx_frame->can_dlc))
{
std::cerr << "ERROR: checksum mismatch, discard frame with id " << rx_frame->can_id << std::endl;
return;
}
// otherwise, update robot state with new frame
TracerMessage status_msg;
DecodeTracerMsgFromCAN(rx_frame, &status_msg);
NewStatusMsgReceivedCallback(status_msg);
}
void TracerBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received)
{
// std::cout << "bytes received from serial: " << bytes_received << std::endl;
// serial_parser_.PrintStatistics();
// serial_parser_.ParseBuffer(buf, bytes_received);
// TODO
// TracerMessage status_msg;
// for (int i = 0; i < bytes_received; ++i)
// {
// if (DecodeTracerMsgFromUART(buf[i], &status_msg))
// NewStatusMsgReceivedCallback(status_msg);
// }
}
void TracerBase::NewStatusMsgReceivedCallback(const TracerMessage &msg)
{
// std::cout << "new status msg received" << std::endl;
std::lock_guard<std::mutex> guard(tracer_state_mutex_);
UpdateTracerState(msg, tracer_state_);
}
void TracerBase::UpdateTracerState(const TracerMessage &status_msg, TracerState &state)
{
switch (status_msg.type)
{
case TracerMotionStatusMsg:
{
// std::cout << "motion control feedback received" << std::endl;
const MotionStatusMessage &msg = status_msg.body.motion_status_msg;
state.linear_velocity = static_cast<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 TracerLightStatusMsg:
{
// std::cout << "light control feedback received" << std::endl;
const LightStatusMessage &msg = status_msg.body.light_status_msg;
if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL)
state.light_control_enabled = false;
else
state.light_control_enabled = true;
state.front_light_state.mode = msg.data.status.front_light_mode;
state.front_light_state.custom_value = msg.data.status.front_light_custom;
state.rear_light_state.mode = msg.data.status.rear_light_mode;
state.rear_light_state.custom_value = msg.data.status.rear_light_custom;
break;
}
case TracerSystemStatusMsg:
{
// std::cout << "system status feedback received" << std::endl;
const SystemStatusMessage &msg = status_msg.body.system_status_msg;
state.control_mode = msg.data.status.control_mode;
state.base_state = msg.data.status.base_state;
state.battery_voltage = (static_cast<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 TracerMotorDriverStatusMsg:
{
// std::cout << "motor 1 driver feedback received" << std::endl;
const MotorDriverStatusMessage &msg = status_msg.body.motor_driver_status_msg;
for (int i = 0; i < 4; ++i)
{
state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].current = (static_cast<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.body.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.body.motor_driver_status_msg.motor_id].temperature = msg.data.status.temperature;
}
break;
}
}
}
} // namespace westonrobot

118
src/platforms/tracer_can_parser.c
View File

@@ -1,118 +0,0 @@
/*
* tracer_can_parser.c
*
* Created on: Apr 14, 2020 10:35
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#include "tracer_protocol/tracer_can_parser.h"
#include "string.h"
static void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame);
bool DecodeTracerMsgFromCAN(const struct can_frame *rx_frame, TracerMessage *msg)
{
msg->type = TracerMsgNone;
switch (rx_frame->can_id)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_STATUS_ID:
{
msg->type = TracerMotionStatusMsg;
memcpy(msg->body.motion_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_SYSTEM_STATUS_STATUS_ID:
{
msg->type = TracerSystemStatusMsg;
memcpy(msg->body.system_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR1_DRIVER_STATUS_ID:
{
msg->type = TracerMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = TRACER_MOTOR1_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
case CAN_MSG_MOTOR2_DRIVER_STATUS_ID:
{
msg->type = TracerMotorDriverStatusMsg;
msg->body.motor_driver_status_msg.motor_id = TRACER_MOTOR2_ID;
memcpy(msg->body.motor_driver_status_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case CAN_MSG_MOTION_CMD_ID:
{
msg->type = TracerMotionCmdMsg;
memcpy(msg->body.motion_cmd_msg.data.raw, rx_frame->data, rx_frame->can_dlc * sizeof(uint8_t));
break;
}
default:
break;
}
return true;
}
void EncodeTracerMsgToCAN(const TracerMessage *msg, struct can_frame *tx_frame)
{
switch (msg->type)
{
// in the current implementation, both MsgType and can_frame include 8 * uint8_t
case TracerMotionStatusMsg:
{
tx_frame->can_id = CAN_MSG_MOTION_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motion_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case TracerSystemStatusMsg:
{
tx_frame->can_id = CAN_MSG_SYSTEM_STATUS_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.system_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case TracerMotorDriverStatusMsg:
{
if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR1_ID)
tx_frame->can_id = CAN_MSG_MOTOR1_DRIVER_STATUS_ID;
else if (msg->body.motor_driver_status_msg.motor_id == TRACER_MOTOR2_ID)
tx_frame->can_id = CAN_MSG_MOTOR2_DRIVER_STATUS_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->body.motor_driver_status_msg.data.raw, tx_frame->can_dlc);
break;
}
case TracerMotionCmdMsg:
{
EncodeTracerMotionControlMsgToCAN(&(msg->body.motion_cmd_msg), tx_frame);
break;
}
default:
break;
}
tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
void EncodeTracerMotionControlMsgToCAN(const MotionCmdMessage *msg, struct can_frame *tx_frame)
{
tx_frame->can_id = CAN_MSG_MOTION_CMD_ID;
tx_frame->can_dlc = 8;
memcpy(tx_frame->data, msg->data.raw, tx_frame->can_dlc);
tx_frame->data[7] = CalcTracerCANChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc);
}
uint8_t CalcTracerCANChecksum(uint16_t id, uint8_t *data, uint8_t dlc)
{
uint8_t checksum = 0x00;
checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc;
for (int i = 0; i < (dlc - 1); ++i)
checksum += data[i];
return checksum;
}

162
src/stopwatch.h
View File

@@ -1,162 +0,0 @@
/*
This is free and unencumbered software released into the public domain.
Anyone is free to copy, modify, publish, use, compile, sell, or
distribute this software, either in source code form or as a compiled
binary, for any purpose, commercial or non-commercial, and by any
means.
In jurisdictions that recognize copyright laws, the author or authors
of this software dedicate any and all copyright interest in the
software to the public domain. We make this dedication for the benefit
of the public at large and to the detriment of our heirs and
successors. We intend this dedication to be an overt act of
relinquishment in perpetuity of all present and future rights to this
software under copyright law.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
OTHER DEALINGS IN THE SOFTWARE.
For more information, please refer to <http://unlicense.org>
*/
#ifndef STOPWATCH_H
#define STOPWATCH_H
#include <array>
#include <chrono>
#include <cstdint>
#include <algorithm>
#include <thread>
namespace westonrobot {
// only supported on x86 processors
#if (defined __x86_64__) || (defined __i386)
// An implementation of the 'TrivialClock' concept using the rdtscp instruction.
struct rdtscp_clock {
using rep = std::uint64_t;
using period = std::ratio<1>;
using duration = std::chrono::duration<rep, period>;
using time_point = std::chrono::time_point<rdtscp_clock, duration>;
static time_point now() noexcept {
std::uint32_t hi, lo;
__asm__ __volatile__("rdtscp" : "=d"(hi), "=a"(lo));
return time_point(duration((static_cast<std::uint64_t>(hi) << 32) | lo));
}
};
// A timer using the specified clock.
template <class Clock = std::chrono::system_clock>
struct timer {
using time_point = typename Clock::time_point;
using duration = typename Clock::duration;
timer(const duration duration) : expiry(Clock::now() + duration) {}
timer(const time_point expiry) : expiry(expiry) {}
bool done() const { return done(Clock::now()); }
bool done(const time_point now) const { return now >= expiry; }
duration remaining() const { return remaining(Clock::now()); };
duration remaining(const time_point now) const { return expiry - now; }
const time_point expiry;
};
template <class Clock = std::chrono::system_clock>
constexpr timer<Clock> make_timer(const typename Clock::duration duration) {
return timer<Clock>(duration);
}
// Times how long it takes a function to execute using the specified clock.
template <class Clock = rdtscp_clock, class Func>
typename Clock::duration time(Func &&function) {
const auto start = Clock::now();
function();
return Clock::now() - start;
}
// Samples the given function N times using the specified clock.
template <std::size_t N, class Clock = rdtscp_clock, class Func>
std::array<typename Clock::duration, N> sample(Func &&function) {
std::array<typename Clock::duration, N> samples;
for (std::size_t i = 0u; i < N; ++i) {
samples[i] = time<Clock>(function);
}
std::sort(samples.begin(), samples.end());
return samples;
}
#endif /* __x86_64__ or __i386 */
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;
};
// toc() in different units
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 westonrobot
#endif // STOPWATCH_H