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

updated scout base to use class MobileBase

Browse Source
This commit is contained in:
Ruixiang Du
2020-07-28 18:30:26 +08:00
parent 981a64b1af
commit 7fcd239c69
3 changed files with 289 additions and 378 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
include/wrp_sdk/platforms/hunter/hunter_base.hpp
View File

@@ -36,10 +36,6 @@ class HunterBase : public MobileBase {
HunterMotionCmd::FaultClearFlag::NO_FAULT); HunterMotionCmd::FaultClearFlag::NO_FAULT);
private: private:
// serial port related variables
uint8_t tx_cmd_len_;
uint8_t tx_buffer_[HUNTER_CMD_BUF_LEN];
// cmd/status update related variables // cmd/status update related variables
std::mutex hunter_state_mutex_; std::mutex hunter_state_mutex_;
std::mutex motion_cmd_mutex_; std::mutex motion_cmd_mutex_;

56
include/wrp_sdk/platforms/scout/scout_base.hpp
View File

@@ -16,40 +16,24 @@
#include <mutex> #include <mutex>
#include <functional> #include <functional>
#include "wrp_sdk/asyncio/async_can.hpp" #include "wrp_sdk/platforms/common/mobile_base.hpp"
#include "wrp_sdk/asyncio/async_serial.hpp"
#include "wrp_sdk/platforms/scout/scout_protocol.h" #include "wrp_sdk/platforms/scout/scout_protocol.h"
#include "wrp_sdk/platforms/scout/scout_can_parser.h" #include "wrp_sdk/platforms/scout/scout_can_parser.h"
#include "wrp_sdk/platforms/scout/scout_uart_parser.h" #include "wrp_sdk/platforms/scout/scout_uart_parser.h"
#include "wrp_sdk/platforms/scout/scout_types.hpp" #include "wrp_sdk/platforms/scout/scout_types.hpp"
namespace westonrobot namespace westonrobot {
{ class ScoutBase : public MobileBase {
class ScoutBase
{
public: public:
ScoutBase() = default; ScoutBase() : MobileBase(){};
~ScoutBase(); ~ScoutBase() = default;
// do not allow copy
ScoutBase(const ScoutBase &scout) = delete;
ScoutBase &operator=(const ScoutBase &scout) = delete;
public: public:
// connect to roboot from CAN or serial
void Connect(std::string dev_name, int32_t baud_rate = 0);
// disconnect from roboot, only valid for serial port
void Disconnect();
// cmd thread runs at 100Hz (10ms) by default
void SetCmdThreadPeriodMs(int32_t period_ms) { cmd_thread_period_ms_ = period_ms; };
// motion control // motion control
void SetMotionCommand(double linear_vel, double angular_vel, void SetMotionCommand(double linear_vel, double angular_vel,
ScoutMotionCmd::FaultClearFlag fault_clr_flag = ScoutMotionCmd::FaultClearFlag::NO_FAULT); ScoutMotionCmd::FaultClearFlag fault_clr_flag =
ScoutMotionCmd::FaultClearFlag::NO_FAULT);
// light control // light control
void SetLightCommand(ScoutLightCmd cmd); void SetLightCommand(ScoutLightCmd cmd);
@@ -59,15 +43,7 @@ public:
ScoutState GetScoutState(); ScoutState GetScoutState();
private: private:
// hardware communication interface // serial port buffer
std::shared_ptr<ASyncCAN> can_if_;
std::shared_ptr<ASyncSerial> serial_if_;
// CAN priority higher than serial if both connected
bool can_connected_ = false;
bool serial_connected_ = false;
// serial port related variables
uint8_t tx_cmd_len_; uint8_t tx_cmd_len_;
uint8_t tx_buffer_[SCOUT_CMD_BUF_LEN]; uint8_t tx_buffer_[SCOUT_CMD_BUF_LEN];
@@ -88,22 +64,18 @@ private:
bool light_ctrl_requested_ = false; bool light_ctrl_requested_ = false;
// internal functions // internal functions
void ConfigureCANBus(const std::string &can_if_name = "can1"); void SendRobotCmd() override;
void ConfigureSerial(const std::string uart_name = "/dev/ttyUSB0", int32_t baud_rate = 115200); void ParseCANFrame(can_frame *rx_frame);
void ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
void StartCmdThread(); size_t bytes_received);
void ControlLoop(int32_t period_ms);
void SendMotionCmd(uint8_t count); void SendMotionCmd(uint8_t count);
void SendLightCmd(uint8_t count); void SendLightCmd(uint8_t count);
void ParseCANFrame(can_frame *rx_frame);
void ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received);
void NewStatusMsgReceivedCallback(const ScoutMessage &msg); void NewStatusMsgReceivedCallback(const ScoutMessage &msg);
public: public:
static void UpdateScoutState(const ScoutMessage &status_msg, ScoutState &state); static void UpdateScoutState(const ScoutMessage &status_msg,
ScoutState &state);
}; };
} // namespace westonrobot } // namespace westonrobot

285
src/platforms/scout_base.cpp
View File

@@ -12,76 +12,15 @@
#include "stopwatch.h" #include "stopwatch.h"
namespace westonrobot namespace westonrobot {
{ void ScoutBase::SendRobotCmd() {
ScoutBase::~ScoutBase() static uint8_t cmd_count = 0;
{ static uint8_t light_cmd_count = 0;
if (serial_connected_) SendMotionCmd(cmd_count++);
serial_if_->close(); if (light_ctrl_requested_) SendLightCmd(light_cmd_count++);
if (cmd_thread_.joinable())
cmd_thread_.join();
} }
void ScoutBase::Connect(std::string dev_name, int32_t baud_rate) void ScoutBase::SendMotionCmd(uint8_t count) {
{
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 // motion control message
ScoutMessage m_msg; ScoutMessage m_msg;
m_msg.type = ScoutMotionControlMsg; m_msg.type = ScoutMotionControlMsg;
@@ -92,9 +31,12 @@ void ScoutBase::SendMotionCmd(uint8_t count)
m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART; m_msg.body.motion_control_msg.data.cmd.control_mode = CTRL_MODE_CMD_UART;
motion_cmd_mutex_.lock(); 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.fault_clear_flag =
m_msg.body.motion_control_msg.data.cmd.linear_velocity_cmd = current_motion_cmd_.linear_velocity; static_cast<uint8_t>(current_motion_cmd_.fault_clear_flag);
m_msg.body.motion_control_msg.data.cmd.angular_velocity_cmd = current_motion_cmd_.angular_velocity; 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(); motion_cmd_mutex_.unlock();
m_msg.body.motion_control_msg.data.cmd.reserved0 = 0; m_msg.body.motion_control_msg.data.cmd.reserved0 = 0;
@@ -102,50 +44,55 @@ void ScoutBase::SendMotionCmd(uint8_t count)
m_msg.body.motion_control_msg.data.cmd.count = count; m_msg.body.motion_control_msg.data.cmd.count = count;
if (can_connected_) 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); m_msg.body.motion_control_msg.data.cmd.checksum =
// serial_connected_: checksum will be calculated later when packed into a complete serial frame 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_) if (can_connected_) {
{
// send to can bus // send to can bus
can_frame m_frame; can_frame m_frame;
EncodeScoutMsgToCAN(&m_msg, &m_frame); EncodeScoutMsgToCAN(&m_msg, &m_frame);
can_if_->send_frame(m_frame); can_if_->send_frame(m_frame);
} } else {
else
{
// send to serial port // send to serial port
EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
serial_if_->send_bytes(tx_buffer_, tx_cmd_len_); serial_if_->send_bytes(tx_buffer_, tx_cmd_len_);
} }
} }
void ScoutBase::SendLightCmd(uint8_t count) void ScoutBase::SendLightCmd(uint8_t count) {
{
ScoutMessage l_msg; ScoutMessage l_msg;
l_msg.type = ScoutLightControlMsg; l_msg.type = ScoutLightControlMsg;
light_cmd_mutex_.lock(); light_cmd_mutex_.lock();
if (light_ctrl_enabled_) 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.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_mode =
l_msg.body.light_control_msg.data.cmd.front_light_custom = current_light_cmd_.front_custom_value; static_cast<uint8_t>(current_light_cmd_.front_mode);
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.front_light_custom =
l_msg.body.light_control_msg.data.cmd.rear_light_custom = current_light_cmd_.rear_custom_value; 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 << " , " // std::cout << "cmd: " << l_msg.data.cmd.front_light_mode << " , " <<
// << l_msg.data.cmd.rear_light_mode << " , " << l_msg.data.cmd.rear_light_custom << std::endl; // 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; // std::cout << "light cmd generated" << std::endl;
} } else {
else l_msg.body.light_control_msg.data.cmd.light_ctrl_enable =
{ LIGHT_DISABLE_CTRL;
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_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.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_mode =
LIGHT_MODE_CONST_OFF;
l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0; l_msg.body.light_control_msg.data.cmd.rear_light_custom = 0;
} }
light_ctrl_requested_ = false; light_ctrl_requested_ = false;
@@ -155,26 +102,27 @@ void ScoutBase::SendLightCmd(uint8_t count)
l_msg.body.light_control_msg.data.cmd.count = count; l_msg.body.light_control_msg.data.cmd.count = count;
if (can_connected_) 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); l_msg.body.light_control_msg.data.cmd.checksum = CalcScoutCANChecksum(
// serial_connected_: checksum will be calculated later when packed into a complete serial frame 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_) if (can_connected_) {
{
// send to can bus // send to can bus
can_frame l_frame; can_frame l_frame;
EncodeScoutMsgToCAN(&l_msg, &l_frame); EncodeScoutMsgToCAN(&l_msg, &l_frame);
can_if_->send_frame(l_frame); can_if_->send_frame(l_frame);
} } else {
else
{
// send to serial port // send to serial port
EncodeScoutMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_); EncodeScoutMsgToUART(&l_msg, tx_buffer_, &tx_cmd_len_);
serial_if_->send_bytes(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) << " , " // std::cout << "cmd: " << static_cast<int>(l_msg.data.cmd.front_light_mode)
// << static_cast<int>(l_msg.data.cmd.rear_light_mode) << " , " << static_cast<int>(l_msg.data.cmd.rear_light_custom) << std::endl; // << " , " << 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: "; // std::cout << "can: ";
// for (int i = 0; i < 8; ++i) // for (int i = 0; i < 8; ++i)
// std::cout << static_cast<int>(l_frame.data[i]) << " "; // std::cout << static_cast<int>(l_frame.data[i]) << " ";
@@ -184,38 +132,16 @@ void ScoutBase::SendLightCmd(uint8_t count)
// std::cout << std::endl; // std::cout << std::endl;
} }
void ScoutBase::ControlLoop(int32_t period_ms) ScoutState ScoutBase::GetScoutState() {
{
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_); std::lock_guard<std::mutex> guard(scout_state_mutex_);
return scout_state_; return scout_state_;
} }
void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel, ScoutMotionCmd::FaultClearFlag fault_clr_flag) 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 // make sure cmd thread is started before attempting to send commands
if (!cmd_thread_started_) if (!cmd_thread_started_) StartCmdThread();
StartCmdThread();
if (linear_vel < ScoutMotionCmd::min_linear_velocity) if (linear_vel < ScoutMotionCmd::min_linear_velocity)
linear_vel = ScoutMotionCmd::min_linear_velocity; linear_vel = ScoutMotionCmd::min_linear_velocity;
@@ -227,15 +153,15 @@ void ScoutBase::SetMotionCommand(double linear_vel, double angular_vel, ScoutMot
angular_vel = ScoutMotionCmd::max_angular_velocity; angular_vel = ScoutMotionCmd::max_angular_velocity;
std::lock_guard<std::mutex> guard(motion_cmd_mutex_); 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_.linear_velocity = static_cast<int8_t>(
current_motion_cmd_.angular_velocity = static_cast<int8_t>(angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0); 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; current_motion_cmd_.fault_clear_flag = fault_clr_flag;
} }
void ScoutBase::SetLightCommand(ScoutLightCmd cmd) void ScoutBase::SetLightCommand(ScoutLightCmd cmd) {
{ if (!cmd_thread_started_) StartCmdThread();
if (!cmd_thread_started_)
StartCmdThread();
std::lock_guard<std::mutex> guard(light_cmd_mutex_); std::lock_guard<std::mutex> guard(light_cmd_mutex_);
current_light_cmd_ = cmd; current_light_cmd_ = cmd;
@@ -243,19 +169,19 @@ void ScoutBase::SetLightCommand(ScoutLightCmd cmd)
light_ctrl_requested_ = true; light_ctrl_requested_ = true;
} }
void ScoutBase::DisableLightCmdControl() void ScoutBase::DisableLightCmdControl() {
{
std::lock_guard<std::mutex> guard(light_cmd_mutex_); std::lock_guard<std::mutex> guard(light_cmd_mutex_);
light_ctrl_enabled_ = false; light_ctrl_enabled_ = false;
light_ctrl_requested_ = true; light_ctrl_requested_ = true;
} }
void ScoutBase::ParseCANFrame(can_frame *rx_frame) void ScoutBase::ParseCANFrame(can_frame *rx_frame) {
{
// validate checksum, discard frame if fails // validate checksum, discard frame if fails
if (!rx_frame->data[7] == CalcScoutCANChecksum(rx_frame->can_id, rx_frame->data, rx_frame->can_dlc)) if (!rx_frame->data[7] == CalcScoutCANChecksum(rx_frame->can_id,
{ rx_frame->data,
std::cerr << "ERROR: checksum mismatch, discard frame with id " << rx_frame->can_id << std::endl; rx_frame->can_dlc)) {
std::cerr << "ERROR: checksum mismatch, discard frame with id "
<< rx_frame->can_id << std::endl;
return; return;
} }
@@ -265,40 +191,45 @@ void ScoutBase::ParseCANFrame(can_frame *rx_frame)
NewStatusMsgReceivedCallback(status_msg); NewStatusMsgReceivedCallback(status_msg);
} }
void ScoutBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize, size_t bytes_received) void ScoutBase::ParseUARTBuffer(uint8_t *buf, const size_t bufsize,
{ size_t bytes_received) {
// std::cout << "bytes received from serial: " << bytes_received << std::endl; // std::cout << "bytes received from serial: " << bytes_received << std::endl;
// serial_parser_.PrintStatistics(); // serial_parser_.PrintStatistics();
// serial_parser_.ParseBuffer(buf, bytes_received); // serial_parser_.ParseBuffer(buf, bytes_received);
ScoutMessage status_msg; ScoutMessage status_msg;
for (int i = 0; i < bytes_received; ++i) for (int i = 0; i < bytes_received; ++i) {
{
if (DecodeScoutMsgFromUART(buf[i], &status_msg)) if (DecodeScoutMsgFromUART(buf[i], &status_msg))
NewStatusMsgReceivedCallback(status_msg); NewStatusMsgReceivedCallback(status_msg);
} }
} }
void ScoutBase::NewStatusMsgReceivedCallback(const ScoutMessage &msg) void ScoutBase::NewStatusMsgReceivedCallback(const ScoutMessage &msg) {
{
// std::cout << "new status msg received" << std::endl; // std::cout << "new status msg received" << std::endl;
std::lock_guard<std::mutex> guard(scout_state_mutex_); std::lock_guard<std::mutex> guard(scout_state_mutex_);
UpdateScoutState(msg, scout_state_); UpdateScoutState(msg, scout_state_);
} }
void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg, ScoutState &state) void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg,
{ ScoutState &state) {
switch (status_msg.type) switch (status_msg.type) {
{ case ScoutMotionStatusMsg: {
case ScoutMotionStatusMsg:
{
// std::cout << "motion control feedback received" << std::endl; // std::cout << "motion control feedback received" << std::endl;
const MotionStatusMessage &msg = status_msg.body.motion_status_msg; 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.linear_velocity =
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; 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; break;
} }
case ScoutLightStatusMsg: case ScoutLightStatusMsg: {
{
// std::cout << "light control feedback received" << std::endl; // std::cout << "light control feedback received" << std::endl;
const LightStatusMessage &msg = status_msg.body.light_status_msg; const LightStatusMessage &msg = status_msg.body.light_status_msg;
if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL) if (msg.data.status.light_ctrl_enable == LIGHT_DISABLE_CTRL)
@@ -311,25 +242,37 @@ void ScoutBase::UpdateScoutState(const ScoutMessage &status_msg, ScoutState &sta
state.rear_light_state.custom_value = msg.data.status.rear_light_custom; state.rear_light_state.custom_value = msg.data.status.rear_light_custom;
break; break;
} }
case ScoutSystemStatusMsg: case ScoutSystemStatusMsg: {
{
// std::cout << "system status feedback received" << std::endl; // std::cout << "system status feedback received" << std::endl;
const SystemStatusMessage &msg = status_msg.body.system_status_msg; const SystemStatusMessage &msg = status_msg.body.system_status_msg;
state.control_mode = msg.data.status.control_mode; state.control_mode = msg.data.status.control_mode;
state.base_state = msg.data.status.base_state; 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.battery_voltage =
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); (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; break;
} }
case ScoutMotorDriverStatusMsg: case ScoutMotorDriverStatusMsg: {
{
// std::cout << "motor 1 driver feedback received" << std::endl; // std::cout << "motor 1 driver feedback received" << std::endl;
const MotorDriverStatusMessage &msg = status_msg.body.motor_driver_status_msg; const MotorDriverStatusMessage &msg =
for (int i = 0; i < ScoutState::motor_num; ++i) 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]
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); .current =
state.motor_states[status_msg.body.motor_driver_status_msg.motor_id].temperature = msg.data.status.temperature; (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; break;
} }