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

updated package structure and more fixes

Browse Source
This commit is contained in:
Ruixiang Du
2019-10-07 23:14:29 +08:00
parent ccccce9d40
commit 829b6f8845
4459 changed files with 6640 additions and 518864 deletions
Download Patch File Download Diff File Expand all files Collapse all files

202
scout_sdk/src/async_io/async_can.cpp Normal file
View File

@@ -0,0 +1,202 @@
/*
* async_can.cpp
*
* Created on: Jun 10, 2019 02:25
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
* Copyright (c) 2016 UAVCAN Team
*/
// This is needed to enable necessary declarations in sys/
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include "scout_sdk/async_io/async_can.hpp"
#include <net/if.h>
#include <poll.h>
#include <string.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <cassert>
#include <iostream>
#include "asyncio_utils.hpp"
using namespace wescore;
using asio::buffer;
using asio::io_service;
using std::error_code;
std::atomic<size_t> ASyncCAN::conn_id_counter{0};
ASyncCAN::ASyncCAN(std::string device) : tx_total_frames(0),
rx_total_frames(0),
last_tx_total_frames(0),
last_rx_total_frames(0),
last_iostat(steady_clock::now()),
io_service(),
stream(io_service)
{
conn_id = conn_id_counter.fetch_add(1);
open(device);
}
ASyncCAN::~ASyncCAN()
{
close();
}
void ASyncCAN::open(std::string device)
{
const size_t iface_name_size = strlen(device.c_str()) + 1;
if (iface_name_size > IFNAMSIZ)
return;
can_fd_ = socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW);
if (can_fd_ < 0)
return;
struct ifreq ifr;
memset(&ifr, 0, sizeof(ifr));
memcpy(ifr.ifr_name, device.c_str(), iface_name_size);
const int ioctl_result = ioctl(can_fd_, SIOCGIFINDEX, &ifr);
if (ioctl_result < 0)
close();
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)
close();
;
can_interface_opened_ = true;
// NOTE: shared_from_this() should not be used in constructors
// give some work to io_service before start
stream.assign(can_fd_);
io_service.post(std::bind(&ASyncCAN::do_read, this, std::ref(rcv_frame), std::ref(stream)));
// run io_service for async io
io_thread = std::thread([this]() {
set_this_thread_name("mcan%zu", conn_id);
io_service.run();
});
}
void ASyncCAN::close()
{
// lock_guard lock(mutex);
// if (!is_open())
// return;
const int close_result = ::close(can_fd_);
can_fd_ = -1;
io_service.stop();
if (io_thread.joinable())
io_thread.join();
io_service.reset();
can_interface_opened_ = false;
if (port_closed_cb)
port_closed_cb();
}
ASyncCAN::IOStat ASyncCAN::get_iostat()
{
std::lock_guard<std::recursive_mutex> lock(iostat_mutex);
IOStat stat;
stat.tx_total_frames = tx_total_frames;
stat.rx_total_frames = rx_total_frames;
auto d_tx = stat.tx_total_frames - last_tx_total_frames;
auto d_rx = stat.rx_total_frames - last_rx_total_frames;
last_tx_total_frames = stat.tx_total_frames;
last_rx_total_frames = stat.rx_total_frames;
auto now = steady_clock::now();
auto dt = now - last_iostat;
last_iostat = now;
float dt_s = std::chrono::duration_cast<std::chrono::seconds>(dt).count();
stat.tx_speed = d_tx / dt_s;
stat.rx_speed = d_rx / dt_s;
return stat;
}
void ASyncCAN::iostat_tx_add(size_t frame)
{
tx_total_frames += frame;
}
void ASyncCAN::iostat_rx_add(size_t frame)
{
rx_total_frames += frame;
}
void ASyncCAN::send_frame(const can_frame &tx_frame)
{
iostat_tx_add(1);
stream.async_write_some(asio::buffer(&tx_frame, sizeof(tx_frame)),
[](error_code error, size_t bytes_transferred) {
// std::cout << "frame sent" << std::endl;
});
}
void ASyncCAN::call_receive_callback(can_frame *rx_frame)
{
// keep track of statistics
iostat_rx_add(1);
// call the actual parser
if (receive_cb)
receive_cb(rx_frame);
else
default_receive_callback(rx_frame);
}
void ASyncCAN::default_receive_callback(can_frame *rx_frame)
{
// do nothing
// std::cerr << "no callback function set" << std::endl;
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::do_read(struct can_frame &rec_frame, asio::posix::basic_stream_descriptor<> &stream)
{
auto sthis = shared_from_this();
stream.async_read_some(
asio::buffer(&rcv_frame, sizeof(rcv_frame)),
[sthis](error_code error, size_t bytes_transferred) {
if (error)
{
std::cerr << "read error in connection " << sthis->conn_id << " : "
<< error.message().c_str() << std::endl;
sthis->close();
return;
}
sthis->call_receive_callback(&sthis->rcv_frame);
sthis->do_read(std::ref(sthis->rcv_frame), std::ref(sthis->stream));
});
}

342
scout_sdk/src/async_io/async_serial.cpp Normal file
View File

@@ -0,0 +1,342 @@
/*
* async_serial.cpp
*
* Created on: Nov 23, 2018 22:24
* Description: asynchronous serial communication using asio
* adapted from code in libmavconn
*
* Main changes: 1. Removed dependency on Boost (asio standalone
* and C++ STL only)
* 2. Removed dependency on console-bridge
* 3. Removed mavlink related code
* 4. Removed UDP/TCP related code
*
* Author: Vladimir Ermakov <vooon341@gmail.com>
* Ruixiang Du <ruixiang.du@gmail.com>
*/
/*
* libmavconn
* Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved.
*
* This file is part of the mavros package and subject to the license terms
* in the top-level LICENSE file of the mavros repository.
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/
#include <cassert>
#include <iostream>
#include "scout_sdk/async_io/async_serial.hpp"
#include "asyncio_utils.hpp"
#if defined(__linux__)
#include <linux/serial.h>
#endif
using namespace wescore;
using asio::buffer;
using asio::io_service;
using std::error_code;
std::atomic<size_t> ASyncSerial::conn_id_counter{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),
last_iostat(steady_clock::now()),
tx_in_progress(false),
tx_q{},
rx_buf{},
io_service(),
serial_dev_(io_service)
{
conn_id = conn_id_counter.fetch_add(1);
}
ASyncSerial::~ASyncSerial()
{
close();
}
void ASyncSerial::open(std::string device, unsigned baudrate, bool hwflow)
{
using SPB = asio::serial_port_base;
if (device != "")
{
device_ = device;
baudrate_ = baudrate;
hwflow_ = hwflow;
}
std::cout << "connection: " << conn_id << " , device: " << device_ << " @ " << baudrate_ << "bps" << std::endl;
try
{
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));
#if defined(__linux__)
// Enable low latency mode on Linux
{
int fd = serial_dev_.native_handle();
struct serial_struct ser_info;
ioctl(fd, TIOCGSERIAL, &ser_info);
ser_info.flags |= ASYNC_LOW_LATENCY;
ioctl(fd, TIOCSSERIAL, &ser_info);
}
#endif
}
catch (std::system_error &err)
{
throw DeviceError("serial", err);
}
// 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));
// run io_service for async io
io_thread = std::thread([this]() {
set_this_thread_name("mserial%zu", conn_id);
io_service.run();
});
}
void ASyncSerial::close()
{
lock_guard lock(mutex);
if (!is_open())
return;
serial_dev_.cancel();
serial_dev_.close();
io_service.stop();
if (io_thread.joinable())
io_thread.join();
io_service.reset();
if (port_closed_cb)
port_closed_cb();
}
ASyncSerial::IOStat ASyncSerial::get_iostat()
{
std::lock_guard<std::recursive_mutex> lock(iostat_mutex);
IOStat stat;
stat.tx_total_bytes = tx_total_bytes;
stat.rx_total_bytes = rx_total_bytes;
auto d_tx = stat.tx_total_bytes - last_tx_total_bytes;
auto d_rx = stat.rx_total_bytes - last_rx_total_bytes;
last_tx_total_bytes = stat.tx_total_bytes;
last_rx_total_bytes = stat.rx_total_bytes;
auto now = steady_clock::now();
auto dt = now - last_iostat;
last_iostat = now;
float dt_s = std::chrono::duration_cast<std::chrono::seconds>(dt).count();
stat.tx_speed = d_tx / dt_s;
stat.rx_speed = d_rx / dt_s;
return stat;
}
void ASyncSerial::iostat_tx_add(size_t bytes)
{
tx_total_bytes += bytes;
}
void ASyncSerial::iostat_rx_add(size_t bytes)
{
rx_total_bytes += bytes;
}
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;
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));
}
void ASyncSerial::call_receive_callback(uint8_t *buf, const std::size_t bufsize, std::size_t bytes_received)
{
assert(bufsize >= bytes_received);
// keep track of statistics
iostat_rx_add(bytes_received);
// call the actual parser
if (receive_cb)
receive_cb(buf, bufsize, bytes_received);
else
default_receive_callback(buf, bufsize, bytes_received);
}
void ASyncSerial::default_receive_callback(uint8_t *buf, const size_t bufsize, size_t bytes_received)
{
// do nothing
std::cerr << "no callback function set" << std::endl;
}
void ASyncSerial::do_read(void)
{
auto sthis = shared_from_this();
serial_dev_.async_read_some(
buffer(rx_buf),
[sthis](error_code error, size_t bytes_transferred) {
if (error)
{
std::cerr << "read error in connection " << sthis->conn_id << " : "
<< error.message().c_str() << std::endl;
sthis->close();
return;
}
sthis->call_receive_callback(sthis->rx_buf.data(), sthis->rx_buf.size(), bytes_transferred);
sthis->do_read();
});
// std::cout << rx_buf.data() << std::endl;
}
void ASyncSerial::do_write(bool check_tx_state)
{
if (check_tx_state && tx_in_progress)
return;
lock_guard lock(mutex);
if (tx_q.empty())
return;
tx_in_progress = true;
auto sthis = shared_from_this();
auto &buf_ref = tx_q.front();
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);
if (error)
{
std::cerr << "write error in connection " << sthis->conn_id << " : "
<< error.message().c_str() << std::endl;
sthis->close();
return;
}
sthis->iostat_tx_add(bytes_transferred);
lock_guard lock(sthis->mutex);
if (sthis->tx_q.empty())
{
sthis->tx_in_progress = false;
return;
}
buf_ref.pos += bytes_transferred;
if (buf_ref.nbytes() == 0)
sthis->tx_q.pop_front();
if (!sthis->tx_q.empty())
sthis->do_write(false);
else
sthis->tx_in_progress = false;
});
}
//---------------------------------------------------------------------------------------//
namespace
{
ASyncSerial::Ptr url_parse_serial(
std::string path, std::string query, bool hwflow)
{
std::string file_path;
int baudrate;
url_parse_host(path, file_path, baudrate, ASyncSerial::DEFAULT_DEVICE, ASyncSerial::DEFAULT_BAUDRATE);
url_parse_query(query);
return std::make_shared<ASyncSerial>(file_path, baudrate, hwflow);
}
} // namespace
ASyncSerial::Ptr ASyncSerial::open_url(std::string url)
{
/* Based on code found here:
* http://stackoverflow.com/questions/2616011/easy-way-to-parse-a-url-in-c-cross-platform
*/
const std::string proto_end("://");
std::string proto;
std::string host;
std::string path;
std::string query;
auto proto_it = std::search(
url.begin(), url.end(),
proto_end.begin(), proto_end.end());
if (proto_it == url.end())
{
// looks like file path
std::cout << "URL: " << url.c_str() << " looks like file path" << std::endl;
return url_parse_serial(url, "", false);
}
// copy protocol
proto.reserve(std::distance(url.begin(), proto_it));
std::transform(url.begin(), proto_it,
std::back_inserter(proto),
std::ref(tolower));
// copy host
std::advance(proto_it, proto_end.length());
auto path_it = std::find(proto_it, url.end(), '/');
std::transform(proto_it, path_it,
std::back_inserter(host),
std::ref(tolower));
// copy path, and query if exists
auto query_it = std::find(path_it, url.end(), '?');
path.assign(path_it, query_it);
if (query_it != url.end())
++query_it;
query.assign(query_it, url.end());
if (proto == "serial")
return url_parse_serial(path, query, false);
else if (proto == "serial-hwfc")
return url_parse_serial(path, query, true);
else
throw DeviceError("url", "Unknown URL type");
}

82
scout_sdk/src/async_io/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

63
scout_sdk/src/async_io/asyncio_utils.hpp Normal file
View File

@@ -0,0 +1,63 @@
/**
* @brief MAVConn async serial utility class
* @file async_utils.hpp
* @author Vladimir Ermakov <vooon341@gmail.com>
*/
/*
* libmavconn
* Copyright 2013,2014,2015,2016 Vladimir Ermakov, All rights reserved.
*
* This file is part of the mavros package and subject to the license terms
* in the top-level LICENSE file of the mavros repository.
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/
#ifndef ASYNCIO_UTILS_HPP
#define ASYNCIO_UTILS_HPP
#include <string>
#include <algorithm>
#include <iostream>
namespace wescore
{
template <typename... Args>
std::string format(const std::string &fmt, Args... args)
{
// C++11 specify that string store elements continously
std::string ret;
auto sz = std::snprintf(nullptr, 0, fmt.c_str(), args...);
ret.reserve(sz + 1);
ret.resize(sz); // to be sure there have room for \0
std::snprintf(&ret.front(), ret.capacity() + 1, fmt.c_str(), args...);
return ret;
}
template <typename... Args>
bool set_this_thread_name(const std::string &name, Args &&... args)
{
auto new_name = format(name, std::forward<Args>(args)...);
#ifdef __APPLE__
return pthread_setname_np(new_name.c_str()) == 0;
#else
pthread_t pth = pthread_self();
return pthread_setname_np(pth, new_name.c_str()) == 0;
#endif
}
/**
* Parse host:port pairs
*/
void url_parse_host(std::string host,
std::string &host_out, int &port_out,
const std::string def_host, const int def_port);
/**
* Parse ?ids=sid,cid
*/
void url_parse_query(std::string query);
} // namespace wescore
#endif /* ASYNCIO_UTILS_HPP */

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

@@ -0,0 +1,391 @@
#include "scout_sdk/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

197
scout_sdk/src/scout_protocol/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_sdk/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_sdk/src/scout_protocol/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_sdk/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;
}