diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 1c1c4fc..6d9848e 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
-
+
@@ -16,7 +16,7 @@
-
+
diff --git a/sdk b/sdk
index 9668eec..22d7d68 160000
--- a/sdk
+++ b/sdk
@@ -1 +1 @@
-Subproject commit 9668eec9f1c1536ed57d72b5a760851455ac5042
+Subproject commit 22d7d685c4a5adaa0e3515a290fdb5c31d594b39
diff --git a/usrc/device.cpp b/usrc/device.cpp
index 6304ed1..2506472 100644
--- a/usrc/device.cpp
+++ b/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;
diff --git a/usrc/device.hpp b/usrc/device.hpp
index 9c0caff..82651da 100644
--- a/usrc/device.hpp
+++ b/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
diff --git a/usrc/driver/preportional_valve_ctrl.cpp b/usrc/driver/preportional_valve_ctrl.cpp
index 04d31a8..e6200b8 100644
--- a/usrc/driver/preportional_valve_ctrl.cpp
+++ b/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);
diff --git a/usrc/driver/preportional_valve_ctrl.hpp b/usrc/driver/preportional_valve_ctrl.hpp
index 061fccd..3baf7d5 100644
--- a/usrc/driver/preportional_valve_ctrl.hpp
+++ b/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
\ No newline at end of file
diff --git a/usrc/main.cpp b/usrc/main.cpp
index 8686adf..cde9b92 100644
--- a/usrc/main.cpp
+++ b/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");
diff --git a/usrc/uart_debug_fn_reg.cpp b/usrc/uart_debug_fn_reg.cpp
index cc127d5..2f7816c 100644
--- a/usrc/uart_debug_fn_reg.cpp
+++ b/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);
});