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

临时版本

Browse Source
This commit is contained in:
murmur 2024-12-29 23:40:50 +08:00
parent 13781723e0
commit c77352d0be
2 changed files with 90 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
protocol.c
View File

@ -200,8 +200,9 @@ static uint8_t writeCMD(uint8_t *txBuf, uint16_t txLen) {
uint8_t rxBuf[30] = {0}; uint8_t rxBuf[30] = {0};
uint16_t rxLen = txLen; uint16_t rxLen = txLen;
readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT); 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个字节应与发送的相同 if(memcmp(rxBuf, txBuf, 2) != 0) {//正常情况下，返回的前2个字节应与发送的相同
log_e("writeCMD error!");
elog_hexdump("writeCMD error", 16, rxBuf, rxLen); elog_hexdump("writeCMD error", 16, rxBuf, rxLen);
systemStatus.rst += 1;//结果计数 systemStatus.rst += 1;//结果计数
return 1; return 1;
@ -712,7 +713,7 @@ uint8_t InitPump(void) {
if (rst != systemStatus.rst) if (rst != systemStatus.rst)
{ {
log_e("InitPump %s failed!", dp.pump[index].name); log_e("InitPump %s failed!", dp.pump[index].name);
systemStatus.ds.initStatus = INIT_FAILED; // systemStatus.ds.initStatus = INIT_FAILED;
return 1; return 1;
} }
} }
@ -779,11 +780,15 @@ uint16_t ReadValve1InputReg(uint8_t id, uint16_t reg)
uint16_t crc = CalculateCRC16(data, 6); uint16_t crc = CalculateCRC16(data, 6);
// 小端序填充 // 小端序填充
memcpy(&data[6], &crc, 2); memcpy(&data[6], &crc, 2);
elog_hexdump("writeCMD", 16, data, sizeof(data));
writeCMD(data, 8); transDataToMotorValve(data, sizeof(data));
uint8_t rxBuf[30] = {0}; uint8_t rxBuf[30] = {0};
uint16_t rxLen = 8; uint16_t rxLen = 7;
uint8_t rst = HAL_UART_Receive(&huart2, rxBuf, rxLen, READ_ACK_TIMEOUT); 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]; 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); uint16_t crc = CalculateCRC16(data, 6);
// 小端序填充 // 小端序填充
memcpy(&data[6], &crc, 2); memcpy(&data[6], &crc, 2);
writeCMD(data, 8); elog_hexdump("writeCMD", 16, data, sizeof(data));
transDataToMotorValve(data, sizeof(data));
uint8_t rxBuf[30] = {0}; uint8_t rxBuf[30] = {0};
uint16_t rxLen = 9; 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]; 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 位置值(用户单位) * @return 位置值(用户单位)
*/ */
static uint32_t ReadValvePos(uint8_t index) { 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.设置原点回归方式 // 1.设置原点回归方式
// (0416h)=37;17=负限位，18=正限位 // (0416h)=37;17=负限位，18=正限位
if(direction > 0) { if(direction > 0) {
log_i("back to Origin, +\r\n"); log_i("[%d]back to Origin, +",index);
SetValveHomeDetectMode(index, 37);//正方向堵转 SetValveHomeDetectMode(index, 37);//正方向堵转
} }
else { else {
log_i("back to Origin, -\r\n"); log_i("[%d]back to Origin, -",index);
SetValveHomeDetectMode(index, 38);//反方向堵转 SetValveHomeDetectMode(index, 38);//反方向堵转
} }
// 2.设置堵转检测力矩和堵转检测时间 // 2.设置堵转检测力矩和堵转检测时间
// (0170h)=300，(0172h)=50 // (0170h)=300，(0172h)=50
SetValveHomeTorque(index, 30);//30% SetValveHomeTorque(index, 30);//30%
SetValveHomeTime(index, 5);//50ms SetValveHomeTime(index, 5);//5ms
// 3.写 (00B1h)=0、运行模式 (03C2h)=0x06，使其工作在原点回归模式； // 3.写 (00B1h)=0、运行模式 (03C2h)=0x06，使其工作在原点回归模式；
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402); SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
SetValveRunMode(index, RTU_VALVE_CFG_MODE_HM); 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); SetValveFunc(index, RTU_VALVE_CFG_RUN_ORIGIN);
if(rst != systemStatus.rst) { if(rst != systemStatus.rst) {
log_e("ValveBackToOrigin[%d] failed!",index); log_e("ValveBackToOrigin[%d] CMD failed!",index);
systemStatus.ds.initStatus = INIT_FAILED; systemStatus.ds.initStatus = INIT_FAILED;
} }
systemStatus.isValveMovingBackToOrigin[index] = 1;
// 2,3,4,5设置过后可不再设置 // 2,3,4,5设置过后可不再设置
// 1,6为必须 // 1,6为必须
@ -1041,22 +1056,34 @@ static void valveCheckBTOResult(uint8_t index)
{ {
static uint8_t retryCnt = 0; static uint8_t retryCnt = 0;
uint8_t isSuccess = 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); 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， // 如果原点回归完成，状态字第12位会从0变为1，
// 如果原点回归失败，状态字第13位会从0变为1。 // 如果原点回归失败，状态字第13位会从0变为1。
// 此外也可以附加判断电机当前位置是否在0附近的200个脉冲以内。 // 此外也可以附加判断电机当前位置是否在0附近的200个脉冲以内。
if(rst & 0x0001<<12) { if((rst>>12) & 0x0001) {
// 成功 // 成功
uint32_t pos = ReadValvePos(index); uint32_t pos = ReadValvePos(index);
if(pos > 200 || pos < (VALVE_PULSE_PER_ROUND-200)) { log_d("valve[%d] bto pos: %d",index,pos);
// 位置超出范围 isSuccess = 1;
isSuccess = 0; SetValveFunc(index, RTU_VALVE_CFG_DISABLE);
} // if(pos > 200 || pos < (VALVE_PULSE_PER_ROUND-200)) {
else { // // 位置超出范围
isSuccess = 1; // isSuccess = 0;
} // }
// else {
// isSuccess = 1;
// }
} }
if(rst & 0x0001<<13) { if((rst>>13) & 0x0001) {
// 失败 // 失败
isSuccess = 0; isSuccess = 0;
} }
@ -1064,17 +1091,20 @@ static void valveCheckBTOResult(uint8_t index)
if (isSuccess) if (isSuccess)
{ {
retryCnt = 0; retryCnt = 0;
systemStatus.ds.initStatus = INIT_SUCCESS; // systemStatus.ds.initStatus = INIT_SUCCESS;
systemStatus.isValveMovingBackToOrigin[index] = 0;
log_i("ValveBackToOrigin[%d] success!",index);
return; return;
} }
else { else {
log_e("ValveBackToOrigin[%d] failed!",index);
retryCnt++; retryCnt++;
if(retryCnt > 2) {//执行两次回归，都失败则认为初始化失败 if(retryCnt > 2) {//执行两次回归，都失败则认为初始化失败
systemStatus.ds.initStatus = INIT_FAILED; systemStatus.ds.initStatus = INIT_FAILED;
retryCnt = 0; retryCnt = 0;
return; return;
} }
ValveBackToOrigin(index, -1); // ValveBackToOrigin(index, -1);
} }
@ -1108,7 +1138,7 @@ uint8_t ValvePPInit(uint8_t index) {
if (rst != systemStatus.rst) if (rst != systemStatus.rst)
{ {
log_e("ValvePPInit[%d] failed!",index); 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) { if(rst != systemStatus.rst) {
log_e("ValveRunToAngle[%d] failed!",index); 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"); printf("InitValve\n");
ValvePPInit(0); ValvePPInit(0);
// ValvePPInit(1); ValvePPInit(1);
} }
void ReadValveSpeedPos(void) void ReadValveSpeedPos(void)
{ {
for(uint8_t index = 0; index < 2; index++) { for(uint8_t index = 0; index < 2; index++) {
systemStatus.valvesSpeed[index] = ReadValveSpeed(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.valvesPos[index] = ReadValvePos(index);
systemStatus.ds.valves.angle[index] = systemStatus.valvesPos[index]*360/dp.valve[index].fullCount; systemStatus.ds.valves.angle[index] = systemStatus.valvesPos[index]*360/dp.valve[index].fullCount;
} }
@ -1172,7 +1202,11 @@ void ReadValveSpeedPos(void)
void updateValveStatus(void) void updateValveStatus(void)
{ {
valveCheckBTOResult(0); 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 //alarm
@ -1218,13 +1252,29 @@ void updateVPInfo(void)
// 获取回归状态 // 获取回归状态
// 获取泵实时速度、位置 // 获取泵实时速度、位置
ReadPumpSpeedPos(); // ReadPumpSpeedPos();
// 获取阀门实时速度、位置 // 获取阀门实时速度、位置
ReadValveSpeedPos(); 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) void updateSystemStatus(void)
{ {
updateVPInfo(); updateVPInfo();
updatePumpStatus(); // updatePumpStatus();
updateValveStatus(); updateValveStatus();
dumpSystemStatus();
} }
/** /**
@ -1246,7 +1297,7 @@ void initCTLSystem(void)
systemStatus.ds.initStatus = INIT_IN_PROGRESS; systemStatus.ds.initStatus = INIT_IN_PROGRESS;
systemStatus.rst = 0; systemStatus.rst = 0;
InitValve(); InitValve();
InitPump(); // InitPump();
} }
@ -1316,7 +1367,7 @@ static uint8_t HandleInit(void) {
if(systemStatus.rst != 0) { if(systemStatus.rst != 0) {
log_e("系统初始化失败"); log_e("系统初始化失败");
systemStatus.ds.initStatus = INIT_FAILED; // systemStatus.ds.initStatus = INIT_FAILED;
return ACK_FAILED; return ACK_FAILED;
} }
return ACK_OK; return ACK_OK;
@ -1632,6 +1683,8 @@ void runPumpDemo(void) {
void runValveDemo(void) { void runValveDemo(void) {
printf("runValveDemo\r\n"); printf("runValveDemo\r\n");
ValveBackToOrigin(0,-1); ValveBackToOrigin(0,-1);
// ValveBackToOrigin(1,-1);
return;
HAL_Delay(5000); HAL_Delay(5000);
// 阀门1正转120度 // 阀门1正转120度
printf("ValveRunToAngle(0, 120)\n"); printf("ValveRunToAngle(0, 120)\n");

7
protocol.h
View File

@ -18,7 +18,7 @@
// 帧头帧尾定义 // 帧头帧尾定义
#define FRAME_HEADER 0xA55A5AA5 #define FRAME_HEADER 0xA55A5AA5
#define FRAME_TAIL 0x5AA5A55A #define FRAME_TAIL 0x5AA5A55A
#define READ_ACK_TIMEOUT 50 #define READ_ACK_TIMEOUT 200
#define ACK_OK 0x0000 #define ACK_OK 0x0000
#define ACK_FAILED 0x0001 #define ACK_FAILED 0x0001
#define ACK_OTHER 0x0002 #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_SC 0x0381 // 阀门运行状态
#define RTU_VALVE_CMD_AL 0x037F // 阀门运行告警 #define RTU_VALVE_CMD_AL 0x037F // 阀门运行告警
#define RTU_VALVE_CMD_POS 0x03C8 // 阀门运行位置，用户单位 #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_MODE 0x0416 // 阀门原点回归方式
#define RTU_VALVE_CMD_HOME_SWT_SPEED 0x0417 // 阀门回归寻找开关的速度 #define RTU_VALVE_CMD_HOME_SWT_SPEED 0x0417 // 阀门回归寻找开关的速度
@ -377,7 +377,7 @@ typedef enum {
// 三通阀结构体 // 三通阀结构体
typedef struct { typedef struct {
uint8_t angle[2]; // 阀门角度 (120/210) uint16_t angle[2]; // 阀门角度 (120/210)
} ValveStatus_t; } ValveStatus_t;
// 泵结构体 // 泵结构体
@ -408,6 +408,7 @@ typedef struct
uint32_t pumpsSpeed[2];//实时速度 uint32_t pumpsSpeed[2];//实时速度
uint8_t pumpsSpeedPercent[2];//实时速度百分比 uint8_t pumpsSpeedPercent[2];//实时速度百分比
uint32_t pumpsPos[2];//实时位置 uint32_t pumpsPos[2];//实时位置
uint8_t isValveMovingBackToOrigin[2];//阀门是否在回归原点
uint16_t rst;//RTU命令执行结果 uint16_t rst;//RTU命令执行结果
} SystemStatus_t;//包含需要上报的状态及附加状态 } SystemStatus_t;//包含需要上报的状态及附加状态