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};
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");

7
protocol.h
View File

@ -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;//包含需要上报的状态及附加状态