Fix preportional valve busy state bug and add

retry mechanism

.settings/language.settings.xml

@@ -5,7 +5,7 @@
 			<provider-reference id="org.eclipse.cdt.ui.UserLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1753696948789538601" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1039985736006445419" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -16,7 +16,7 @@
 			<provider-reference id="org.eclipse.cdt.ui.UserLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1801727843789013388" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="991954841006970632" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>

sdk

@@ -1 +1 @@
-Subproject commit 9668eec9f1c1536ed57d72b5a760851455ac5042
+Subproject commit 22d7d685c4a5adaa0e3515a290fdb5c31d594b39

usrc/device.cpp

@@ -203,18 +203,14 @@ int32_t preportional_valve_is_busy(int32_t *busy) {
   int32_t valve1state = 0;
   int32_t valve2state = 0;
   int32_t err         = 0;
-  *busy               = 0;
+  *busy               = 1;
 
-  err = m_PreportionalValveHost.getValveWorkState(1, &valve1state);
+  err = m_PreportionalValveHost.isBusy(1, &valve1state);
   if (err != 0) return err;
-  err = m_PreportionalValveHost.getValveWorkState(2, &valve2state);
+  err = m_PreportionalValveHost.isBusy(2, &valve2state);
   if (err != 0) return err;
-  if (valve1state == m_PreportionalValveHost.kstate_err_state || valve2state == m_PreportionalValveHost.kstate_err_state) {
-    *busy = 0;
-    return 1;
-  }
 
-  if (valve1state == m_PreportionalValveHost.kstate_stop && valve2state == m_PreportionalValveHost.kstate_stop) {
+  if (valve1state == 0 && valve2state == 0) {
     *busy = 0;
   } else {
     *busy = 1;

usrc/device.hpp

@@ -64,6 +64,7 @@ void air_compressor_valve1_set(int32_t val);
 void air_compressor_valve2_set(int32_t val);
 void air_compressor_read_pressure(int32_t *ack);
 void triple_warning_light_ctl(uint8_t r, uint8_t g, uint8_t b, uint8_t warning);
+
 int32_t preportional_valve_is_busy(int32_t *busy);
 
 };  // namespace iflytop

usrc/driver/preportional_valve_ctrl.cpp

@@ -8,8 +8,24 @@ using namespace iflytop;
 void PreportionalValveCtrl::initialize(UART_HandleTypeDef* huart) { m_modbusBlockHost.initialize(huart); }
 
 int32_t PreportionalValveCtrl::setValvePos(int32_t valueid, int32_t pos) {  //
+
+  /**
+   * @brief 发送失败重发3次
+   */
+  for (size_t i = 0; i < 3; i++) {
+    int32_t err = _setValvePos(valueid, pos);
+    if (err == 0) return 0;
+  }
+  return 1;
+}
+int32_t PreportionalValveCtrl::_setValvePos(int32_t valueid, int32_t pos) {  //
+  if (valueid > 255 || valueid < 1) {
+    return 1;
+  }
   bool ret = m_modbusBlockHost.writeReg06(valueid, CTRL_STATE_REG, pos, 30);
   if (!ret) return 1;
+  m_last_set_valve_ticket = HAL_GetTick();
+  m_targetpos[valueid]    = pos;
   return 0;
 }
 int32_t PreportionalValveCtrl::getValvePos(int32_t valueid, int32_t* pos) {
@@ -20,6 +36,65 @@ int32_t PreportionalValveCtrl::getValvePos(int32_t valueid, int32_t* pos) {
   if (!ret) return 1;
   return 0;
 }
+int32_t PreportionalValveCtrl::getValveOrderPos(int32_t valueid, int32_t* pos) {
+  uint16_t pos16 = 0;
+  bool     ret   = m_modbusBlockHost.readReg03(valueid, CTRL_STATE_REG, &pos16, 30);
+  *pos           = pos16;
+  if (!ret) return 1;
+  return 0;
+}
+#define TAG "PreportionalValveCtrl"
+
+int32_t PreportionalValveCtrl::isBusy(int32_t valueid, int32_t* busy) {
+#if 1
+  int32_t orderpos = 0;
+  int32_t pos      = 0;
+  int32_t err      = 0;
+
+#if 0
+  err = getValveOrderPos(valueid, &orderpos);
+  if (err != 0) return err;
+#endif
+  orderpos = m_targetpos[valueid];
+
+  err = getValvePos(valueid, &pos);
+  if (err != 0) return err;
+
+  if (abs(m_targetpos[valueid] - pos) <= 11) {
+    *busy = 0;
+  } else {
+    *busy = 1;
+  }
+  return 0;
+#endif
+
+#if 0
+
+  /**
+   * @brief
+   * 比例阀当前状态的更新是有延迟的，例如下发指令控制比例阀开合到90%,下发执行后（收到指令的回执），
+   * 此时读取比例阀状态，比例阀的状态并非运动中，而是stop，需要等待一段时间后，再读取状态，状态才是运动中。
+   */
+  if (haspassedms(m_last_set_valve_ticket) < 300) {
+    *busy = 1;
+    return 0;
+  }
+  int32_t valve1state = 0;
+  int32_t err         = 0;
+
+  err = getValveWorkState(1, &valve1state);
+  if (err != 0) return err;
+
+  if (valve1state == kstate_stop) {
+    *busy = 0;
+  } else {
+    *busy = 1;
+  }
+  return 0;
+
+#endif
+}
+
 int32_t PreportionalValveCtrl::getValveWorkState(int32_t valueid, int32_t* state) {
   uint16_t state16 = 0;
   bool     ret     = m_modbusBlockHost.readReg03(valueid, WORK_STATE_REG, &state16, 30);

usrc/driver/preportional_valve_ctrl.hpp

@@ -29,6 +29,9 @@ class PreportionalValveCtrl {
  private:
   /* data */
   ModbusBlockHost m_modbusBlockHost;
+  int32_t         val                     = 0;
+  uint32_t        m_last_set_valve_ticket = 0;
+  uint16_t        m_targetpos[255];
 
  public:
   PreportionalValveCtrl(){};
@@ -37,7 +40,12 @@ class PreportionalValveCtrl {
   void    initialize(UART_HandleTypeDef* huart);
   int32_t setValvePos(int32_t valueid, int32_t pos);
   int32_t getValvePos(int32_t valueid, int32_t* pos);
+  int32_t getValveOrderPos(int32_t valueid, int32_t* pos);
+  int32_t isBusy(int32_t valueid, int32_t* busy);
   int32_t getValveWorkState(int32_t valueid, int32_t* state);
+
+ private:
+  int32_t _setValvePos(int32_t valueid, int32_t pos);
 };
 
 }  // namespace iflytop

usrc/main.cpp

@@ -15,7 +15,7 @@ void umain(void) {
   ZHALCORE::getInstance()->initialize(oscfg);
 
   ZLOGI(TAG, "pipeline_disinfection_liquid_path_control:%s", VERSION);
-
+  device_init();
   uart_debug_fn_reg();
   can_cmd_reg_fn_reg();
   ZLOGI(TAG, "init done");

usrc/uart_debug_fn_reg.cpp

@@ -53,8 +53,8 @@ void uart_debug_fn_reg() {
   });
 
   cmdScheduler.regCMD("set_proportional_valve", "(uint8_t id,uint8_t val)", 2, [](int32_t paramN, const char **paraV, ICmdParserACK *ack) {
-    uint8_t id  = atoi(paraV[0]);
-    uint8_t val = atoi(paraV[1]);
+    int32_t id  = atoi(paraV[0]);
+    int32_t val = atoi(paraV[1]);
     m_PreportionalValveHost.setValvePos(id, val);
     ack->setNoneAck(0);
   });