update

components/mini_servo_motor/mini_servo_motor_ctrl_module.cpp

@@ -55,10 +55,29 @@ int32_t MiniRobotCtrlModule::read_version(version_t& version) {
   return 0;
 }
 int32_t MiniRobotCtrlModule::read_status(status_t& status) {
-  
-  
-   return 0; }
-int32_t MiniRobotCtrlModule::read_debug_info(debug_info_t& debug_info) { return 0; }
+  if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
+
+  FeiTeServoMotor::detailed_status_t feitestatus;
+  if (!m_bus->read_detailed_status(m_id, &feitestatus)) return err::kce_subdevice_overtime;
+  status.torque   = feitestatus.torque;
+  status.speed    = feitestatus.vel;
+  status.pos      = feitestatus.pos;
+  status.io_state = 0;
+  if (feitestatus.state != 0) {
+    status.status = 3;
+  } else {
+    status.status = m_thread.isworking();
+  }
+  return 0;
+}
+int32_t MiniRobotCtrlModule::read_debug_info(detailed_status_t& debug_info) {
+  if (!m_bus->ping(m_id)) return err::kce_subdevice_overtime;
+  FeiTeServoMotor::status_t feitestatus;
+  if (!m_bus->read_status(m_id, &feitestatus)) return err::kce_subdevice_overtime;
+
+
+  return 0;
+}
 
 int32_t MiniRobotCtrlModule::set_run_param(run_param_t& param) { return 0; }
 int32_t MiniRobotCtrlModule::set_basic_param(basic_param_t& param) { return 0; }
@@ -67,3 +86,4 @@ int32_t MiniRobotCtrlModule::set_warning_limit_param(warning_limit_param_t& para
 int32_t MiniRobotCtrlModule::get_run_param(run_param_t& param) { return 0; }
 int32_t MiniRobotCtrlModule::get_basic_param(basic_param_t& param) { return 0; }
 int32_t MiniRobotCtrlModule::get_warning_limit_param(warning_limit_param_t& param) { return 0; }
+uint8_t MiniRobotCtrlModule::get_module_status() { ; }

components/mini_servo_motor/mini_servo_motor_ctrl_module.hpp

@@ -10,7 +10,7 @@ class MiniRobotCtrlModule : public I_MiniServoModule {
   FeiTeServoMotor* m_bus;
   uint8_t          m_id;
   ZThread          m_thread;
-  s32 m_pos_shift;
+  s32              m_pos_shift;
 
  public:
   void initialize(FeiTeServoMotor* bus, uint8_t id);
@@ -27,7 +27,7 @@ class MiniRobotCtrlModule : public I_MiniServoModule {
 
   virtual int32_t read_version(version_t& version) override;
   virtual int32_t read_status(status_t& status) override;
-  virtual int32_t read_debug_info(debug_info_t& debug_info) override;
+  virtual int32_t read_debug_info(detailed_status_t& debug_info) override;
 
   virtual int32_t set_run_param(run_param_t& param) override;
   virtual int32_t set_basic_param(basic_param_t& param) override;
@@ -36,5 +36,8 @@ class MiniRobotCtrlModule : public I_MiniServoModule {
   virtual int32_t get_run_param(run_param_t& param) override;
   virtual int32_t get_basic_param(basic_param_t& param) override;
   virtual int32_t get_warning_limit_param(warning_limit_param_t& param) override;
+
+ private:
+  uint8_t get_module_status();
 };
 }  // namespace iflytop

components/step_motor_ctrl_module/step_motor_ctrl_module.cpp

@@ -208,7 +208,7 @@ int32_t StepMotorCtrlModule::read_status(status_t& status) {
   getnowpos(status.x);
   return 0;
 }
-int32_t StepMotorCtrlModule::read_debug_info(debug_info_t& debug_info) {
+int32_t StepMotorCtrlModule::read_debug_info(detailed_status_t& debug_info) {
   debug_info.status = m_status;
   if (m_Xgpio) debug_info.io_state |= m_Xgpio->getState() ? 0x01 : 0x00;
   if (m_end_gpio) debug_info.io_state |= m_end_gpio->getState() ? 0x02 : 0x00;

components/step_motor_ctrl_module/step_motor_ctrl_module.hpp

@@ -58,7 +58,7 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule {
 
   virtual int32_t read_version(version_t& version) override;
   virtual int32_t read_status(status_t& status) override;
-  virtual int32_t read_debug_info(debug_info_t& debug_info) override;
+  virtual int32_t read_debug_info(detailed_status_t& debug_info) override;
 
   virtual int32_t set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) override;
   virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override;

components/xy_robot_ctrl_module/xy_robot_ctrl_module.cpp

@@ -199,7 +199,7 @@ int32_t XYRobotCtrlModule::read_status(status_t& status) {
   getnowpos(status.x, status.y);
   return 0;
 }
-int32_t XYRobotCtrlModule::read_debug_info(debug_info_t& debug_info) {
+int32_t XYRobotCtrlModule::read_debug_info(detailed_status_t& debug_info) {
   zlock_guard lock(m_lock);
   debug_info = {0};
   if (m_Xgpio) debug_info.iostate |= m_Xgpio->getState() ? 0x01 : 0x00;

components/xy_robot_ctrl_module/xy_robot_ctrl_module.hpp

@@ -70,7 +70,7 @@ class XYRobotCtrlModule : public I_XYRobotCtrlModule {
 
   virtual int32_t read_version(version_t& version) override;
   virtual int32_t read_status(status_t& status) override;
-  virtual int32_t read_debug_info(debug_info_t& debug_info) override;
+  virtual int32_t read_debug_info(detailed_status_t& debug_info) override;
 
   virtual int32_t set_run_param(uint8_t operation, const run_param_t& param, run_param_t& ack) override;
   virtual int32_t set_run_to_zero_param(uint8_t operation, const run_to_zero_param_t& param, run_to_zero_param_t& ack) override;

components/zcancmder_module/zcan_step_motor_ctrl_module.cpp

@@ -95,7 +95,7 @@ void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
   }
   END_PP();
 
-  PROCESS_PACKET(kcmd_step_motor_ctrl_read_debug_info, m_id) {  //
+  PROCESS_PACKET(kcmd_step_motor_ctrl_read_detailed_status, m_id) {  //
     errorcode = m_module->read_debug_info(ack->ack);
   }
   END_PP();

components/zcancmder_module/zcan_xy_robot_module.cpp

@@ -86,7 +86,7 @@ void ZCANXYRobotCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) {
   }
   END_PP();
 
-  PROCESS_PACKET(kcmd_xy_robot_ctrl_read_debug_info, m_id) {  //
+  PROCESS_PACKET(kcmd_xy_robot_ctrl_read_detailed_status, m_id) {  //
     errorcode = m_xyRobotCtrlModule->read_debug_info(ack->ack);
   }
   END_PP();

components/zprotocols/zcancmder

@@ -1 +1 @@
-Subproject commit 8a33d6eeef9b51f409f6364235c7dbc26556fab5
+Subproject commit 2393d97dec8c2e235087f1eaf239e5ad4ebb9f2f

os/zthread.cpp

@@ -7,24 +7,25 @@ using namespace std;
 static void zosthread_default_task(void const *argument) {
   ZThread *thread = (ZThread *)argument;
   ZASSERT(thread);
-  thread->m_threadisworkingFlag        = false;
-  thread->m_threadisWaitingForStopFlag = false;
+  thread->threadcb();
+};
+void ZThread::threadcb() {
+  m_status = kidle;
 
   while (true) {
-    if (thread->m_threadisworkingFlagCallSide) {
-      thread->m_threadisworkingFlag = true;
-      thread->m_taskfunction();
+    if (m_threadisworkingFlagCallSide) {
+      m_status = kworking;
+      m_taskfunction();
+      m_status = kdead;
 
-      thread->m_threadisWaitingForStopFlag = true;
-      while (thread->m_threadisworkingFlagCallSide) {
+      while (m_threadisworkingFlagCallSide) {
         vTaskDelay(10);
       }
-      thread->m_threadisworkingFlag        = false;
-      thread->m_threadisWaitingForStopFlag = false;
+      m_status = kidle;
     }
     vTaskDelay(10);
   }
-};
+}
 
 void ZThread::init(const char *threadname, int stack_size, osPriority priority) {
   int r_task_create = 0;
@@ -52,7 +53,7 @@ void ZThread::start(zosthread_cb_t cb) {
   xSemaphoreTake(m_lock, portMAX_DELAY);
   m_threadisworkingFlagCallSide = true;
   // xEventGroupSetBits(m_zthreadstartworkevent, 0x01);
-  while (!m_threadisworkingFlag) {
+  while (m_status != kidle) {
     xTaskNotifyGive(m_defaultTaskHandle);
     vTaskDelay(1);
   }
@@ -63,7 +64,7 @@ void ZThread::stop() {
   xSemaphoreTake(m_lock, portMAX_DELAY);
   m_threadisworkingFlagCallSide = false;
   // xEventGroupSetBits(m_zthreadstartworkevent, 0x01);
-  while (m_threadisworkingFlag) {
+  while (m_status != kidle) {
     xTaskNotifyGive(m_defaultTaskHandle);
     vTaskDelay(1);
   }

os/zthread.hpp

@@ -6,12 +6,21 @@ typedef function<void()> zosthread_cb_t;
 
 class ZThread {
  public:
+  typedef enum {
+    kidle,
+    kworking,
+    kdead,
+  } status_t;
+
   const char* m_name;
   size_t      m_stacksize;
   osPriority  m_uxPriority;
 
+  status_t m_status = kidle;
+#if 0
   bool m_threadisworkingFlag         = false;
   bool m_threadisWaitingForStopFlag  = false;
+#endif
   bool m_threadisworkingFlagCallSide = false;
 
   zosthread_cb_t     m_taskfunction;
@@ -27,9 +36,13 @@ class ZThread {
   void stop();
 
   bool getExitFlag() { return !m_threadisworkingFlagCallSide; }
+  bool isworking() { return m_status == kworking; }
 
   void sleep(uint32_t ms);
   void wake();
+
+ public:
+  void threadcb();
 };
 
 }  // namespace iflytop