diff --git a/include/wrp_sdk/platforms/common/mobile_base.hpp b/include/wrp_sdk/platforms/common/mobile_base.hpp index 6e12a6a..b50d4d5 100644 --- a/include/wrp_sdk/platforms/common/mobile_base.hpp +++ b/include/wrp_sdk/platforms/common/mobile_base.hpp @@ -56,7 +56,7 @@ class MobileBase { bool enable_timeout_ = true; uint32_t timeout_ms_ = 100; uint32_t watchdog_counter_ = 0; - virtual void CheckCmdTimeout(); + void FeedCmdTimeoutWatchdog() { watchdog_counter_ = 0; }; // command thread std::thread cmd_thread_; diff --git a/src/asyncio/async_can.cpp b/src/asyncio/async_can.cpp index ba99ba2..8a9abef 100644 --- a/src/asyncio/async_can.cpp +++ b/src/asyncio/async_can.cpp @@ -107,7 +107,7 @@ void AsyncCAN::SendFrame(const can_frame &frame) { asio::buffer(&frame, sizeof(frame)), [](asio::error_code error, size_t bytes_transferred) { if (error) { - std::cerr << "Failed to send CAN frame"; + std::cerr << "Failed to send CAN frame" << std::endl; } // std::cout << "frame sent" << std::endl; }); diff --git a/src/platforms/hunter_base.cpp b/src/platforms/hunter_base.cpp index a23503b..17fcc52 100644 --- a/src/platforms/hunter_base.cpp +++ b/src/platforms/hunter_base.cpp @@ -17,6 +17,7 @@ namespace westonrobot { void HunterBase::SendRobotCmd() { static uint8_t cmd_count = 0; SendMotionCmd(cmd_count++); + FeedCmdTimeoutWatchdog(); } void HunterBase::SendMotionCmd(uint8_t count) { diff --git a/src/platforms/mobile_base.cpp b/src/platforms/mobile_base.cpp index 4088020..bb75e3e 100644 --- a/src/platforms/mobile_base.cpp +++ b/src/platforms/mobile_base.cpp @@ -67,9 +67,12 @@ void MobileBase::StartCmdThread() { void MobileBase::ControlLoop(int32_t period_ms) { StopWatch ctrl_sw; bool print_loop_freq = false; + if (timeout_ms_ < period_ms) timeout_ms_ = period_ms; + uint32_t timeout_iter_num = static_cast(timeout_ms_ / period_ms); while (true) { ctrl_sw.tic(); - SendRobotCmd(); + if (watchdog_counter_ < timeout_iter_num) SendRobotCmd(); + ++watchdog_counter_; ctrl_sw.sleep_until_ms(period_ms); if (print_loop_freq) std::cout << "control loop frequency: " << 1.0 / ctrl_sw.toc() diff --git a/src/platforms/scout_base.cpp b/src/platforms/scout_base.cpp index 6e78c99..0d32e41 100644 --- a/src/platforms/scout_base.cpp +++ b/src/platforms/scout_base.cpp @@ -17,6 +17,7 @@ void ScoutBase::SendRobotCmd() { static uint8_t cmd_count = 0; static uint8_t light_cmd_count = 0; SendMotionCmd(cmd_count++); + FeedCmdTimeoutWatchdog(); if (light_ctrl_requested_) SendLightCmd(light_cmd_count++); }