murmur/injectorCTL
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update HandlePumpTimeControl, ReadPumpSpeedPos

Browse Source
This commit is contained in:
murmur 2025-01-02 23:44:05 +08:00
parent f6453f4811
commit b64f12b88e
1 changed files with 48 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
protocol.c
View File

@ -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: