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

saved work on cmd timeout, not tested yet

Browse Source
This commit is contained in:
Ruixiang Du
2020-09-14 18:57:27 +08:00
parent 8ba694b894
commit 9e646c78b7
5 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
include/wrp_sdk/platforms/common/mobile_base.hpp
View File

@@ -56,7 +56,7 @@ class MobileBase {
bool enable_timeout_ = true; bool enable_timeout_ = true;
uint32_t timeout_ms_ = 100; uint32_t timeout_ms_ = 100;
uint32_t watchdog_counter_ = 0; uint32_t watchdog_counter_ = 0;
virtual void CheckCmdTimeout(); void FeedCmdTimeoutWatchdog() { watchdog_counter_ = 0; };
// command thread // command thread
std::thread cmd_thread_; std::thread cmd_thread_;

2
src/asyncio/async_can.cpp
View File

@@ -107,7 +107,7 @@ void AsyncCAN::SendFrame(const can_frame &frame) {
asio::buffer(&frame, sizeof(frame)), asio::buffer(&frame, sizeof(frame)),
[](asio::error_code error, size_t bytes_transferred) { [](asio::error_code error, size_t bytes_transferred) {
if (error) { if (error) {
std::cerr << "Failed to send CAN frame"; std::cerr << "Failed to send CAN frame" << std::endl;
} }
// std::cout << "frame sent" << std::endl; // std::cout << "frame sent" << std::endl;
}); });

1
src/platforms/hunter_base.cpp
View File

@@ -17,6 +17,7 @@ namespace westonrobot {
void HunterBase::SendRobotCmd() { void HunterBase::SendRobotCmd() {
static uint8_t cmd_count = 0; static uint8_t cmd_count = 0;
SendMotionCmd(cmd_count++); SendMotionCmd(cmd_count++);
FeedCmdTimeoutWatchdog();
} }
void HunterBase::SendMotionCmd(uint8_t count) { void HunterBase::SendMotionCmd(uint8_t count) {

5
src/platforms/mobile_base.cpp
View File

@@ -67,9 +67,12 @@ 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 = static_cast<uint32_t>(timeout_ms_ / period_ms);
while (true) { while (true) {
ctrl_sw.tic(); ctrl_sw.tic();
SendRobotCmd(); if (watchdog_counter_ < timeout_iter_num) SendRobotCmd();
++watchdog_counter_;
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()

1
src/platforms/scout_base.cpp
View File

@@ -17,6 +17,7 @@ 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++);
} }