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

added cmd timeout logic

Browse Source
This commit is contained in:
Ruixiang Du
2020-09-15 09:56:26 +08:00
parent c501b6a38d
commit 0887261a99
2 changed files with 19 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/platforms/mobile_base.cpp
View File

@@ -67,12 +67,24 @@ void MobileBase::StartCmdThread() {
void MobileBase::ControlLoop(int32_t period_ms) { void MobileBase::ControlLoop(int32_t period_ms) {
StopWatch ctrl_sw; StopWatch ctrl_sw;
bool print_loop_freq = false; bool print_loop_freq = false;
if (timeout_ms_ < period_ms) timeout_ms_ = period_ms; uint32_t timeout_iter_num;
uint32_t timeout_iter_num = static_cast<uint32_t>(timeout_ms_ / period_ms);
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) { while (true) {
ctrl_sw.tic(); ctrl_sw.tic();
if (watchdog_counter_ < timeout_iter_num) SendRobotCmd(); if (enable_timeout_) {
++watchdog_counter_; ++watchdog_counter_;
if (watchdog_counter_ < timeout_iter_num)
SendRobotCmd();
else
std::cout << "Warning: cmd timeout, not sent to robot" << std::endl;
} else {
SendRobotCmd();
}
ctrl_sw.sleep_until_ms(period_ms); ctrl_sw.sleep_until_ms(period_ms);
if (print_loop_freq) if (print_loop_freq)
std::cout << "control loop frequency: " << 1.0 / ctrl_sw.toc() std::cout << "control loop frequency: " << 1.0 / ctrl_sw.toc()

4
src/platforms/scout_base.cpp
View File

@@ -17,8 +17,8 @@ void ScoutBase::SendRobotCmd() {
static uint8_t cmd_count = 0; static uint8_t cmd_count = 0;
static uint8_t light_cmd_count = 0; static uint8_t light_cmd_count = 0;
SendMotionCmd(cmd_count++); SendMotionCmd(cmd_count++);
FeedCmdTimeoutWatchdog();
if (light_ctrl_requested_) SendLightCmd(light_cmd_count++); if (light_ctrl_requested_) SendLightCmd(light_cmd_count++);
FeedCmdTimeoutWatchdog();
} }
void ScoutBase::SendMotionCmd(uint8_t count) { void ScoutBase::SendMotionCmd(uint8_t count) {
@@ -56,10 +56,12 @@ void ScoutBase::SendMotionCmd(uint8_t count) {
can_frame m_frame; can_frame m_frame;
EncodeScoutMsgToCAN(&m_msg, &m_frame); EncodeScoutMsgToCAN(&m_msg, &m_frame);
can_if_->SendFrame(m_frame); can_if_->SendFrame(m_frame);
std::cout << "CAN cmd sent" << std::endl;
} 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_->SendBytes(tx_buffer_, tx_cmd_len_); serial_if_->SendBytes(tx_buffer_, tx_cmd_len_);
std::cout << "serial cmd sent" << std::endl;
} }
} }