From 0887261a990b07921798fbaf819a7e22645e6e80 Mon Sep 17 00:00:00 2001
From: Ruixiang Du <ruixiang.du@westonrobot.com>
Date: Tue, 15 Sep 2020 09:56:26 +0800
Subject: [PATCH] added cmd timeout logic

---
 src/platforms/mobile_base.cpp | 20 ++++++++++++++++----
 src/platforms/scout_base.cpp  |  4 +++-
 2 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/src/platforms/mobile_base.cpp b/src/platforms/mobile_base.cpp
index bb75e3e..3bbf875 100644
--- a/src/platforms/mobile_base.cpp
+++ b/src/platforms/mobile_base.cpp
@@ -67,12 +67,24 @@ 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<uint32_t>(timeout_ms_ / period_ms);
+  uint32_t timeout_iter_num;
+
+  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) {
     ctrl_sw.tic();
-    if (watchdog_counter_ < timeout_iter_num) SendRobotCmd();
-    ++watchdog_counter_;
+    if (enable_timeout_) {
+      ++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);
     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 0d32e41..52a8ab9 100644
--- a/src/platforms/scout_base.cpp
+++ b/src/platforms/scout_base.cpp
@@ -17,8 +17,8 @@ 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++);
+  FeedCmdTimeoutWatchdog();
 }
 
 void ScoutBase::SendMotionCmd(uint8_t count) {
@@ -56,10 +56,12 @@ void ScoutBase::SendMotionCmd(uint8_t count) {
     can_frame m_frame;
     EncodeScoutMsgToCAN(&m_msg, &m_frame);
     can_if_->SendFrame(m_frame);
+    std::cout << "CAN cmd sent" << std::endl;
   } else {
     // send to serial port
     EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_);
     serial_if_->SendBytes(tx_buffer_, tx_cmd_len_);
+    std::cout << "serial cmd sent" << std::endl;
   }
 }