diff --git a/protocol.c b/protocol.c index 6b1b050..bbcef03 100644 --- a/protocol.c +++ b/protocol.c @@ -43,8 +43,8 @@ SystemStatus_t systemStatus = { DeviceParam_t dp = { .pump = { - {"pump1", 4, 60, 100, 100,40000,0}, - {"pump2", 3, 60, 100, 100,40000,0} + {"pump1", 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; - readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT); + uint16_t rxLen = 7; + 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; - readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT*2); + uint16_t rxLen = 9; + 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: