update HandlePumpTimeControl, ReadPumpSpeedPos

protocol.c

@@ -43,8 +43,8 @@ SystemStatus_t systemStatus = {
 
 DeviceParam_t dp = {
     .pump = {
-        {"pump1", 4, 60, 100, 100,40000,0},
+        {"pump1", 3, 60, 100, 100,40000,0},
-        {"pump2", 3, 60, 100, 100,40000,0}
+        {"pump2", 4, 60, 100, 100,40000,0}
     },
     .valve = {
         {"valve1", 1, 10, 100, 100,47620,0},
@@ -330,8 +330,14 @@ uint16_t ReadPump1Reg(uint8_t id, uint16_t reg) {
 
     writeCMD(data, 8);
     uint8_t rxBuf[30] = {0};
-    uint16_t rxLen = 8;
+    uint16_t rxLen = 7;
-    readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT);
+    uint8_t ret = readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT);
+    if(memcmp(rxBuf,data,2) != 0) {
+        log_e("READ ACK ERROR:[%s]", ackError[ret]);
+        elog_hexdump("GET DATA", 16, rxBuf, rxLen);
+        systemStatus.rst += 1;//结果计数
+        return 1;
+    }
     return rxBuf[3]<<8|rxBuf[4];
 }
 
@@ -342,7 +348,7 @@ uint16_t ReadPump1Reg(uint8_t id, uint16_t reg) {
  * @param return 寄存器值
  */
 uint32_t ReadPump2Reg(uint8_t id, uint16_t reg) {
-    uint8_t data[12] = {0};
+    uint8_t data[8] = {0};
     data[0] = id;
     data[1] = RTU_FUNC_READ_HOLD_REG;
     FillBigEndian16(&data[2], reg);
@@ -352,12 +358,18 @@ uint32_t ReadPump2Reg(uint8_t id, uint16_t reg) {
     // 小端序填充
     memcpy(&data[6], &crc, 2);
 
-    writeCMD(data, 12);
+    writeCMD(data, sizeof(data));
     uint8_t rxBuf[30] = {0};
-    uint16_t rxLen = 12;
+    uint16_t rxLen = 9;
-    readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT*2);
+    uint8_t ret = readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT*10);
+    if(memcmp(rxBuf,data,2) != 0) {
+        log_e("READ ACK ERROR:[%s]", ackError[ret]);
+        elog_hexdump("GET DATA", 16, rxBuf, rxLen);
+        systemStatus.rst += 1;//结果计数
+        return 1;
+    }
     // 逻辑需进一步完善
-    return rxBuf[6]<<24|rxBuf[7]<<16|rxBuf[8]<<8|rxBuf[9];
+    return rxBuf[3]<<24|rxBuf[4]<<16|rxBuf[5]<<8|rxBuf[6];
 }
 
 /*
@@ -633,7 +645,7 @@ uint16_t ReadPumpAlarm(uint8_t index) {
  * @param reg4001 告警寄存器值
  */
 void DecodePumpAlarmMsg(uint16_t reg4001) {
-    printf("reg4001: %x\r\n", reg4001);
+    log_d("reg4001: %x\r\n", reg4001);
     static AlarmCode_t alarmCode = {0};
     // 与上次告警信息相同，则不更新，仅打印一次
     if (alarmCode.all == reg4001)
@@ -702,7 +714,7 @@ void ReadPumpSpeedPos(void)
         systemStatus.pumpsPos[index] = pos;
         log_d("%s pos = %d",dp.pump[index].name,pos);
 
-        uint16_t speed =  ReadPump2Reg(dp.pump[index].id,RTU_PUMP_CMD_SPEED);
+        uint16_t speed =  ReadPump1Reg(dp.pump[index].id,RTU_PUMP_CMD_SPEED);
         systemStatus.pumpsSpeed[index] = speed;
         systemStatus.pumpsSpeedPercent[index] = (uint8_t)(speed * 100 / dp.pump[index].fullSpeed);
         log_d("%s speed = %d",dp.pump[index].name,speed);
@@ -749,10 +761,10 @@ uint8_t InitPump(void) {
 
         SetPumpJogAcc(index, dp.pump[index].accel);
         SetPumpJogDec(index, dp.pump[index].decel);
-        SetPumpJogSpeed(index, dp.pump[index].fullSpeed*dp.pump[index].speedPercent/100);
+        SetPumpJogSpeed(index, systemStatus.pumpsSpeed[index]);
         SetPumpStepAcc(index, dp.pump[index].accel);
         SetPumpStepDec(index, dp.pump[index].decel);
-        SetPumpStepSpeed(index, dp.pump[index].fullSpeed*dp.pump[index].speedPercent/100);
+        SetPumpStepSpeed(index, systemStatus.pumpsSpeed[index]);
         if (rst != systemStatus.rst)
         {
             log_e("InitPump %s failed!", dp.pump[index].name);
@@ -1381,7 +1393,8 @@ void dumpSystemStatus(void)
 void updateSystemStatus(void)
 {
     updateVPInfo();
-    // updatePumpStatus();
+    ReadPumpSpeedPos();
+    updatePumpStatus();
     updateValveStatus();
     dumpSystemStatus();
 }
@@ -1566,18 +1579,21 @@ static uint8_t HandlePumpTimeControl(uint8_t *Buff, uint8_t len) {
             direction = -1;
         }
         uint16_t time = (Buff[index*3+1]<<8) | Buff[index*3+2];
+        log_d("泵时长控制: %d[%d], [%ds]",index, dp.pump[index].id, time);
         if(time == 0) {
             StopPump(index);
             // The jogging direction is set by the last DI command.
             SetPumpStepTarget(index,direction);
             StartPumpJog(index);
-            continue;
+            // continue;
+        }
+        else {
+            // 使用步数方式更靠谱，通过时间和速度计算步数，结束时不用发送停止命令
+            int32_t step = direction*time*systemStatus.pumpsSpeed[index]*PUMP_SPEED_RPS;
+            StopPump(index);//stop first
+            SetPumpStepTarget(index, step);
+            StartPumpRelativeMove(index);
         }
-        // 使用步数方式更靠谱，通过时间和速度计算步数，结束时不用发送停止命令
-        int32_t step = direction*time*systemStatus.pumpsSpeed[index];
-        StopPump(index);//stop first
-        SetPumpStepTarget(index, step);
-        StartPumpRelativeMove(index);
 
         if(rst != systemStatus.rst) {
             log_e("泵时长控制错误");
@@ -1736,6 +1752,12 @@ CmdFrameError_t checkHostCmd(uint8_t *rxBuf, uint8_t rxLen) {
 }
 
 
+void resetACK(uint16_t error) {
+    if(error == ACK_FAILED) {
+        systemStatus.rst = 0;
+    }
+}
+
 /**
  * 处理上位机发送的命令
  * 采用自定义协议,非modbus协议
@@ -1763,26 +1785,32 @@ void ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen) {
             break;
         case HOST_CMD_VALVE_CTRL:
             error = HandleValveControl(data, dataLen);
+            resetACK(error);
             packMsgToHost(HOST_CMD_VALVE_CTRL, error);
             break;
         case HOST_CMD_PUMP_RUN_TIME:
             error = HandlePumpTimeControl(data, dataLen);
+            resetACK(error);
             packMsgToHost(HOST_CMD_PUMP_RUN_TIME, error);
             break;
         case HOST_CMD_PUMP_RUN_SPEED:
             error = HandlePumpSpeedControl(data, dataLen);
+            resetACK(error);
             packMsgToHost(HOST_CMD_PUMP_RUN_SPEED, error);
             break;
         case HOST_CMD_SOFT_STOP:
             error = HandleSoftStop(data, dataLen);
+            resetACK(error);
             packMsgToHost(HOST_CMD_SOFT_STOP, error);
             break;
         case HOST_CMD_PUMP_RUN_STEP:
             error = HandlePumpStepControl(data, dataLen);
+            resetACK(error);
             packMsgToHost(HOST_CMD_PUMP_RUN_STEP, error);
             break;
         case HOST_CMD_SYSTEM_INIT:
             error = HandleInit();
+            resetACK(error);
             packMsgToHost(HOST_CMD_SYSTEM_INIT, error);
             break;
         default: