From c77352d0bec4f4495d4c9d2e55195de4494a403f Mon Sep 17 00:00:00 2001 From: murmur Date: Sun, 29 Dec 2024 23:40:50 +0800 Subject: [PATCH] =?UTF-8?q?=E4=B8=B4=E6=97=B6=E7=89=88=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- protocol.c | 119 ++++++++++++++++++++++++++++++++++++++--------------- protocol.h | 7 ++-- 2 files changed, 90 insertions(+), 36 deletions(-) diff --git a/protocol.c b/protocol.c index 3826852..2f5f73e 100644 --- a/protocol.c +++ b/protocol.c @@ -200,8 +200,9 @@ static uint8_t writeCMD(uint8_t *txBuf, uint16_t txLen) { uint8_t rxBuf[30] = {0}; uint16_t rxLen = txLen; readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT); + // HAL_UART_Receive(&huart2, rxBuf, rxLen, READ_ACK_TIMEOUT*4); + // elog_hexdump("ACK", 16, rxBuf, rxLen); if(memcmp(rxBuf, txBuf, 2) != 0) {//正常情况下，返回的前2个字节应与发送的相同 - log_e("writeCMD error!"); elog_hexdump("writeCMD error", 16, rxBuf, rxLen); systemStatus.rst += 1;//结果计数 return 1; @@ -712,7 +713,7 @@ uint8_t InitPump(void) { if (rst != systemStatus.rst) { log_e("InitPump %s failed!", dp.pump[index].name); - systemStatus.ds.initStatus = INIT_FAILED; + // systemStatus.ds.initStatus = INIT_FAILED; return 1; } } @@ -779,11 +780,15 @@ uint16_t ReadValve1InputReg(uint8_t id, uint16_t reg) uint16_t crc = CalculateCRC16(data, 6); // 小端序填充 memcpy(&data[6], &crc, 2); - - writeCMD(data, 8); + elog_hexdump("writeCMD", 16, data, sizeof(data)); + transDataToMotorValve(data, sizeof(data)); uint8_t rxBuf[30] = {0}; - uint16_t rxLen = 8; - uint8_t rst = HAL_UART_Receive(&huart2, rxBuf, rxLen, READ_ACK_TIMEOUT); + uint16_t rxLen = 7; + readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT*5); + if(memcmp(rxBuf, data, 2) != 0) { + elog_hexdump("ReadValve1InputReg error!", 16, rxBuf, rxLen); + return 0xffff; + } return rxBuf[3]<<8|rxBuf[4]; } @@ -806,11 +811,16 @@ uint32_t ReadValve2InputReg(uint8_t id, uint16_t reg) uint16_t crc = CalculateCRC16(data, 6); // 小端序填充 memcpy(&data[6], &crc, 2); - writeCMD(data, 8); + elog_hexdump("writeCMD", 16, data, sizeof(data)); + transDataToMotorValve(data, sizeof(data)); uint8_t rxBuf[30] = {0}; uint16_t rxLen = 9; - uint8_t rst = HAL_UART_Receive(&huart2, rxBuf, rxLen, READ_ACK_TIMEOUT); + readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT*2); + if(memcmp(rxBuf, data, 2) != 0) { + elog_hexdump("ReadValve2InputReg error!", 16, rxBuf, rxLen); + return 0xffffffff; + } return rxBuf[3]<<24|rxBuf[4]<<16|rxBuf[5]<<8|rxBuf[6]; } @@ -967,7 +977,11 @@ static uint8_t SetValveHomeTime(uint8_t index, uint16_t time) { * @return 位置值(用户单位) */ static uint32_t ReadValvePos(uint8_t index) { - return ReadValve2InputReg(dp.valve[index].id, RTU_VALVE_CMD_POS); + int32_t pos = ReadValve2InputReg(dp.valve[index].id, RTU_VALVE_CMD_POS); + if(pos < 0) { + pos = 0; + } + return pos; } /** @@ -994,17 +1008,17 @@ void ValveBackToOrigin(uint8_t index,int8_t direction) { // 1.设置原点回归方式 // (0416h)=37;17=负限位，18=正限位 if(direction > 0) { - log_i("back to Origin, +\r\n"); + log_i("[%d]back to Origin, +",index); SetValveHomeDetectMode(index, 37);//正方向堵转 } else { - log_i("back to Origin, -\r\n"); + log_i("[%d]back to Origin, -",index); SetValveHomeDetectMode(index, 38);//反方向堵转 } // 2.设置堵转检测力矩和堵转检测时间 // (0170h)=300，(0172h)=50 SetValveHomeTorque(index, 30);//30% - SetValveHomeTime(index, 5);//50ms + SetValveHomeTime(index, 5);//5ms // 3.写 (00B1h)=0、运行模式 (03C2h)=0x06，使其工作在原点回归模式； SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402); SetValveRunMode(index, RTU_VALVE_CFG_MODE_HM); @@ -1021,9 +1035,10 @@ void ValveBackToOrigin(uint8_t index,int8_t direction) { SetValveFunc(index, RTU_VALVE_CFG_RUN_ORIGIN); if(rst != systemStatus.rst) { - log_e("ValveBackToOrigin[%d] failed!",index); + log_e("ValveBackToOrigin[%d] CMD failed!",index); systemStatus.ds.initStatus = INIT_FAILED; } + systemStatus.isValveMovingBackToOrigin[index] = 1; // 2,3,4,5设置过后可不再设置 // 1,6为必须 @@ -1041,22 +1056,34 @@ static void valveCheckBTOResult(uint8_t index) { static uint8_t retryCnt = 0; uint8_t isSuccess = 0; + if (systemStatus.ds.initStatus != INIT_IN_PROGRESS && systemStatus.isValveMovingBackToOrigin[index] == 1) {//初始化中才检查 + return; + } + uint16_t rst = ReadValve1InputReg(dp.valve[index].id,RTU_VALVE_CMD_SC); + if(rst == 0xffff) { + log_e("error to read valve[%d] bto rst",index); + return; + } + log_d("valve[%d] bto rst: 0x%04X, %d, %d",index,rst,(rst>>12)&0x0001,(rst>>13)&0x0001); // 如果原点回归完成，状态字第12位会从0变为1， // 如果原点回归失败，状态字第13位会从0变为1。 // 此外也可以附加判断电机当前位置是否在0附近的200个脉冲以内。 - if(rst & 0x0001<<12) { + if((rst>>12) & 0x0001) { // 成功 uint32_t pos = ReadValvePos(index); - if(pos > 200 || pos < (VALVE_PULSE_PER_ROUND-200)) { - // 位置超出范围 - isSuccess = 0; - } - else { - isSuccess = 1; - } + log_d("valve[%d] bto pos: %d",index,pos); + isSuccess = 1; + SetValveFunc(index, RTU_VALVE_CFG_DISABLE); + // if(pos > 200 || pos < (VALVE_PULSE_PER_ROUND-200)) { + // // 位置超出范围 + // isSuccess = 0; + // } + // else { + // isSuccess = 1; + // } } - if(rst & 0x0001<<13) { + if((rst>>13) & 0x0001) { // 失败 isSuccess = 0; } @@ -1064,17 +1091,20 @@ static void valveCheckBTOResult(uint8_t index) if (isSuccess) { retryCnt = 0; - systemStatus.ds.initStatus = INIT_SUCCESS; + // systemStatus.ds.initStatus = INIT_SUCCESS; + systemStatus.isValveMovingBackToOrigin[index] = 0; + log_i("ValveBackToOrigin[%d] success!",index); return; } else { + log_e("ValveBackToOrigin[%d] failed!",index); retryCnt++; if(retryCnt > 2) {//执行两次回归，都失败则认为初始化失败 systemStatus.ds.initStatus = INIT_FAILED; retryCnt = 0; return; } - ValveBackToOrigin(index, -1); + // ValveBackToOrigin(index, -1); } @@ -1108,7 +1138,7 @@ uint8_t ValvePPInit(uint8_t index) { if (rst != systemStatus.rst) { log_e("ValvePPInit[%d] failed!",index); - systemStatus.ds.initStatus = INIT_FAILED; + // systemStatus.ds.initStatus = INIT_FAILED; } } @@ -1139,7 +1169,7 @@ uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) { if(rst != systemStatus.rst) { log_e("ValveRunToAngle[%d] failed!",index); - systemStatus.ds.initStatus = INIT_FAILED; + // systemStatus.ds.initStatus = INIT_FAILED; } } @@ -1153,14 +1183,14 @@ uint8_t InitValve(void) { printf("InitValve\n"); ValvePPInit(0); - // ValvePPInit(1); + ValvePPInit(1); } void ReadValveSpeedPos(void) { for(uint8_t index = 0; index < 2; index++) { systemStatus.valvesSpeed[index] = ReadValveSpeed(index); - // systemStatus.valvesSpeedPercent[index] = transSpeedToSpeedPercent(index, systemStatus.valvesSpeed[index]); + systemStatus.valvesSpeedPercent[index] = transSpeedToSpeedPercent(index, systemStatus.valvesSpeed[index]); systemStatus.valvesPos[index] = ReadValvePos(index); systemStatus.ds.valves.angle[index] = systemStatus.valvesPos[index]*360/dp.valve[index].fullCount; } @@ -1172,7 +1202,11 @@ void ReadValveSpeedPos(void) void updateValveStatus(void) { valveCheckBTOResult(0); - valveCheckBTOResult(1); + // valveCheckBTOResult(1); + if (systemStatus.ds.initStatus == INIT_IN_PROGRESS && systemStatus.isValveMovingBackToOrigin[0] == 0 && systemStatus.isValveMovingBackToOrigin[1] == 0) { + systemStatus.ds.initStatus = INIT_SUCCESS; + } + //alarm @@ -1218,13 +1252,29 @@ void updateVPInfo(void) // 获取回归状态 // 获取泵实时速度、位置 - ReadPumpSpeedPos(); + // ReadPumpSpeedPos(); // 获取阀门实时速度、位置 ReadValveSpeedPos(); } +/** + * 打印系统状态 + */ +void dumpSystemStatus(void) +{ + log_d("----------------"); + log_d("valve pos: %d[%d%] / %d[%d]",systemStatus.valvesPos[0], \ + systemStatus.ds.valves.angle[0], \ + systemStatus.valvesPos[1], \ + systemStatus.ds.valves.angle[1]); + log_d("valve speed: %d / %d",systemStatus.valvesSpeed[0],systemStatus.valvesSpeed[1]); + log_d("pump pos: %d / %d",systemStatus.pumpsPos[0],systemStatus.pumpsPos[1]); + log_d("pump speed: %d / %d",systemStatus.pumpsSpeed[0],systemStatus.pumpsSpeed[1]); + log_d("----------------"); +} + //在主循环中调用 /** * 更新系统的所有状态数据 @@ -1232,8 +1282,9 @@ void updateVPInfo(void) void updateSystemStatus(void) { updateVPInfo(); - updatePumpStatus(); + // updatePumpStatus(); updateValveStatus(); + dumpSystemStatus(); } /** @@ -1246,7 +1297,7 @@ void initCTLSystem(void) systemStatus.ds.initStatus = INIT_IN_PROGRESS; systemStatus.rst = 0; InitValve(); - InitPump(); + // InitPump(); } @@ -1316,7 +1367,7 @@ static uint8_t HandleInit(void) { if(systemStatus.rst != 0) { log_e("系统初始化失败"); - systemStatus.ds.initStatus = INIT_FAILED; + // systemStatus.ds.initStatus = INIT_FAILED; return ACK_FAILED; } return ACK_OK; @@ -1632,6 +1683,8 @@ void runPumpDemo(void) { void runValveDemo(void) { printf("runValveDemo\r\n"); ValveBackToOrigin(0,-1); + // ValveBackToOrigin(1,-1); + return; HAL_Delay(5000); // 阀门1正转120度 printf("ValveRunToAngle(0, 120)\n"); diff --git a/protocol.h b/protocol.h index 1b3d238..07e81f2 100644 --- a/protocol.h +++ b/protocol.h @@ -18,7 +18,7 @@ // 帧头帧尾定义 #define FRAME_HEADER 0xA55A5AA5 #define FRAME_TAIL 0x5AA5A55A -#define READ_ACK_TIMEOUT 50 +#define READ_ACK_TIMEOUT 200 #define ACK_OK 0x0000 #define ACK_FAILED 0x0001 #define ACK_OTHER 0x0002 @@ -305,7 +305,7 @@ static const uint8_t statusInfo[16][60]={ #define RTU_VALVE_CMD_SC 0x0381 // 阀门运行状态 #define RTU_VALVE_CMD_AL 0x037F // 阀门运行告警 #define RTU_VALVE_CMD_POS 0x03C8 // 阀门运行位置，用户单位 -#define RTU_VALVE_CMD_SPEED 0x03D5 // 阀门当前速度，用户单位/s，rpm +#define RTU_VALVE_CMD_SPEED 0x03D0 // 阀门当前速度，用户单位/s，0x03D5=rpm #define RTU_VALVE_CMD_HOME_MODE 0x0416 // 阀门原点回归方式 #define RTU_VALVE_CMD_HOME_SWT_SPEED 0x0417 // 阀门回归寻找开关的速度 @@ -377,7 +377,7 @@ typedef enum { // 三通阀结构体 typedef struct { - uint8_t angle[2]; // 阀门角度 (120/210) + uint16_t angle[2]; // 阀门角度 (120/210) } ValveStatus_t; // 泵结构体 @@ -408,6 +408,7 @@ typedef struct uint32_t pumpsSpeed[2];//实时速度 uint8_t pumpsSpeedPercent[2];//实时速度百分比 uint32_t pumpsPos[2];//实时位置 + uint8_t isValveMovingBackToOrigin[2];//阀门是否在回归原点 uint16_t rst;//RTU命令执行结果 } SystemStatus_t;//包含需要上报的状态及附加状态