update logic， now valve is OK
This commit is contained in:
murmur
parent
219aea0cfe
commit
22bee679a8
122
protocol.c
122
protocol.c
@ -36,11 +36,11 @@ SystemStatus_t systemStatus = {
|
||||
.pumpsSpeed = {0},
|
||||
.pumpsPos = {0},
|
||||
.pumpsSpeedPercent = {0},
|
||||
.rst = 0
|
||||
.rst = 0,
|
||||
.isValveMovingBackToOrigin = {0},
|
||||
.isBTOOk = {0}
|
||||
};
|
||||
|
||||
uint8_t isValveMovingBackToOrigin[2] = {0,0};
|
||||
|
||||
DeviceParam_t dp = {
|
||||
.pump = {
|
||||
{"pump1", 4, 60, 100, 100,40000,0},
|
||||
@ -52,7 +52,7 @@ DeviceParam_t dp = {
|
||||
},
|
||||
.sensor = {
|
||||
{169, 254, 1, 1}, // 设置IP为169.254.1.1
|
||||
"/index.xml" // 设置路径为/index.xml
|
||||
"/status.xml" // 设置路径为/index.xml
|
||||
}
|
||||
};
|
||||
|
||||
@ -160,6 +160,15 @@ uint16_t CalculateCRC16(uint8_t *data, uint16_t length) {
|
||||
// deviceStatus.bubbleStatus = value;
|
||||
// }
|
||||
|
||||
/**
|
||||
* 更新活动计数器
|
||||
*
|
||||
* @param value 活动计数器值
|
||||
*/
|
||||
void updateActivityMeter(float value) {
|
||||
deviceStatus.activityMeter = value;
|
||||
}
|
||||
|
||||
/**
|
||||
* 更新急停状态
|
||||
*
|
||||
@ -186,7 +195,7 @@ uint16_t CalculateCRC16(uint8_t *data, uint16_t length) {
|
||||
// void updateInitStatus(InitStatus_t status) {
|
||||
// deviceStatus.initStatus = status;
|
||||
// }
|
||||
static uint8_t motorACKLen = 0;
|
||||
volatile uint8_t motorACKLen = 0;
|
||||
void processReceivedData(uint8_t len)
|
||||
{
|
||||
motorACKLen = len;
|
||||
@ -207,14 +216,14 @@ static uint8_t writeCMD(uint8_t *txBuf, uint16_t txLen) {
|
||||
elog_hexdump("writeCMD", 16, txBuf, txLen);
|
||||
|
||||
transDataToMotorValve(txBuf, txLen);
|
||||
// uint8_t rxBuf[30] = {0};
|
||||
// uint16_t rxLen = txLen;
|
||||
// uint8_t ret = readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT);
|
||||
|
||||
// valve反馈时间不定，超时概率极高
|
||||
#ifdef MOTOR_USE_DMA
|
||||
// 阻塞接收，无超时，可能卡死
|
||||
while (!motorACKLen)
|
||||
{
|
||||
/* code */
|
||||
HAL_Delay(10);
|
||||
log_d("wait motorACKLen");
|
||||
}
|
||||
|
||||
if(memcmp(motorRxBuf,txBuf,2) != 0) {
|
||||
@ -228,6 +237,24 @@ static uint8_t writeCMD(uint8_t *txBuf, uint16_t txLen) {
|
||||
systemStatus.rst += 0;//结果计数
|
||||
return 0;
|
||||
}
|
||||
#else
|
||||
|
||||
uint8_t rxBuf[30] = {0};
|
||||
uint16_t rxLen = txLen;
|
||||
uint8_t ret = readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT);
|
||||
if(memcmp(rxBuf,txBuf,2) != 0) {
|
||||
log_e("READ ACK ERROR:[%s]", ackError[ret]);
|
||||
elog_hexdump("GET DATA", 16, rxBuf, rxLen);
|
||||
systemStatus.rst += 1;//结果计数
|
||||
return 1;
|
||||
}
|
||||
else {
|
||||
// log_i("writeCMD success!");
|
||||
systemStatus.rst += 0;//结果计数
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
@ -773,11 +800,30 @@ uint8_t InitPump(void) {
|
||||
*/
|
||||
|
||||
// 与pump通用
|
||||
uint8_t (*writeValve1Reg)(uint8_t index, uint16_t reg, uint16_t value) = WritePump1Reg;
|
||||
// uint8_t (*writeValve1Reg)(uint8_t index, uint16_t reg, uint16_t value) = WritePump1Reg;
|
||||
uint8_t (*writeValve2Reg)(uint8_t index, uint16_t reg, uint32_t value) = WritePump2Reg;
|
||||
uint8_t (*readValve1Reg)(uint8_t index, uint16_t reg) = ReadPump1Reg;
|
||||
uint8_t (*readValve2Reg)(uint8_t index, uint16_t reg) = ReadPump2Reg;
|
||||
|
||||
|
||||
// 写阀门1个寄存器
|
||||
uint8_t writeValve1Reg(uint8_t id, uint16_t reg, int16_t value) {
|
||||
// 写一个寄存器不需要指定寄存器长度
|
||||
uint8_t data[11] = {0};
|
||||
data[0] = id;
|
||||
data[1] = RTU_FUNC_WRITE_MULTI_REG;
|
||||
FillBigEndian16(&data[2], reg);
|
||||
FillBigEndian16(&data[4], 1);
|
||||
data[6] = 2;
|
||||
FillBigEndian16(&data[7], value);
|
||||
uint16_t crc = CalculateCRC16(data, 9);
|
||||
// 小端序填充
|
||||
memcpy(&data[9], &crc, 2);
|
||||
|
||||
return writeCMD(data, 11);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 读取阀门1个输入寄存器
|
||||
*
|
||||
@ -789,7 +835,7 @@ uint16_t ReadValve1InputReg(uint8_t id, uint16_t reg)
|
||||
{
|
||||
uint8_t data[8] = {0};
|
||||
data[0] = id;
|
||||
data[1] = RTU_FUNC_READ_INPUT_REG;
|
||||
data[1] = RTU_FUNC_READ_HOLD_REG;
|
||||
FillBigEndian16(&data[2], reg);
|
||||
FillBigEndian16(&data[4], 1);
|
||||
|
||||
@ -800,9 +846,10 @@ uint16_t ReadValve1InputReg(uint8_t id, uint16_t reg)
|
||||
transDataToMotorValve(data, sizeof(data));
|
||||
uint8_t rxBuf[30] = {0};
|
||||
uint16_t rxLen = 7;
|
||||
readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT*5);
|
||||
uint8_t hrst = readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT);
|
||||
if(memcmp(rxBuf, data, 2) != 0) {
|
||||
elog_hexdump("ReadValve1InputReg error!", 16, rxBuf, rxLen);
|
||||
log_e("READ ACK ERROR:[%s]", ackError[hrst]);
|
||||
elog_hexdump("GET DATA", 16, rxBuf, rxLen);
|
||||
return 0xffff;
|
||||
}
|
||||
return rxBuf[3]<<8|rxBuf[4];
|
||||
@ -832,9 +879,10 @@ uint32_t ReadValve2InputReg(uint8_t id, uint16_t reg)
|
||||
|
||||
uint8_t rxBuf[30] = {0};
|
||||
uint16_t rxLen = 9;
|
||||
readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT*2);
|
||||
uint8_t hrst = readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT*2);
|
||||
if(memcmp(rxBuf, data, 2) != 0) {
|
||||
elog_hexdump("ReadValve2InputReg error!", 16, rxBuf, rxLen);
|
||||
log_e("READ ACK ERROR:[%s]", ackError[hrst]);
|
||||
elog_hexdump("GET DATA", 16, rxBuf, rxLen);
|
||||
return 0xffffffff;
|
||||
}
|
||||
return rxBuf[3]<<24|rxBuf[4]<<16|rxBuf[5]<<8|rxBuf[6];
|
||||
@ -1021,9 +1069,11 @@ static uint32_t ReadValveSpeed(uint8_t index) {
|
||||
*/
|
||||
void ValveBackToOrigin(uint8_t index,int8_t direction) {
|
||||
uint8_t rst = systemStatus.rst;
|
||||
systemStatus.isValveMovingBackToOrigin[index] = 1;
|
||||
systemStatus.isBTOOk[index] = 0;
|
||||
|
||||
// 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);
|
||||
|
||||
// 1.设置原点回归方式
|
||||
@ -1074,15 +1124,24 @@ void ValveBackToOrigin(uint8_t index,int8_t direction) {
|
||||
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) {//初始化中才检查
|
||||
uint8_t isSuccess = 2;
|
||||
if (!systemStatus.isValveMovingBackToOrigin[index]) {//初始化中才检查
|
||||
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;
|
||||
uint32_t pos = ReadValvePos(index);
|
||||
log_d("valve[%d] bto pos: %d",index,pos);
|
||||
if(pos < 10) {
|
||||
rst = 0;
|
||||
isSuccess = 1;
|
||||
}
|
||||
else {
|
||||
return;
|
||||
}
|
||||
|
||||
}
|
||||
log_d("valve[%d] bto rst: 0x%04X, %d, %d",index,rst,(rst>>12)&0x0001,(rst>>13)&0x0001);
|
||||
// 如果原点回归完成，状态字第12位会从0变为1，
|
||||
@ -1090,8 +1149,8 @@ static void valveCheckBTOResult(uint8_t index)
|
||||
// 此外也可以附加判断电机当前位置是否在0附近的200个脉冲以内。
|
||||
if((rst>>12) & 0x0001) {
|
||||
// 成功
|
||||
uint32_t pos = ReadValvePos(index);
|
||||
log_d("valve[%d] bto pos: %d",index,pos);
|
||||
// uint32_t pos = ReadValvePos(index);
|
||||
// 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)) {
|
||||
@ -1107,27 +1166,28 @@ static void valveCheckBTOResult(uint8_t index)
|
||||
isSuccess = 0;
|
||||
}
|
||||
|
||||
if (isSuccess)
|
||||
if (isSuccess == 1)
|
||||
{
|
||||
retryCnt = 0;
|
||||
// systemStatus.ds.initStatus = INIT_SUCCESS;
|
||||
systemStatus.isValveMovingBackToOrigin[index] = 0;
|
||||
systemStatus.isBTOOk[index] = 1;
|
||||
log_i("ValveBackToOrigin[%d] success!",index);
|
||||
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
|
||||
// SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
|
||||
SetValveRunMode(index, RTU_VALVE_CFG_MODE_PP);
|
||||
return;
|
||||
}
|
||||
else {
|
||||
log_e("ValveBackToOrigin[%d] failed!",index);
|
||||
else if(isSuccess == 0) {
|
||||
retryCnt++;
|
||||
log_e("Valve[%d] BackToOrigin failed, [%d]",index,retryCnt);
|
||||
if(retryCnt > 2) {//执行两次回归，都失败则认为初始化失败
|
||||
systemStatus.ds->initStatus = INIT_FAILED;
|
||||
retryCnt = 0;
|
||||
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
|
||||
// SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
|
||||
SetValveRunMode(index, RTU_VALVE_CFG_MODE_PP);
|
||||
return;
|
||||
}
|
||||
// ValveBackToOrigin(index, -1);
|
||||
ValveBackToOrigin(index, -1);
|
||||
}
|
||||
|
||||
|
||||
@ -1175,6 +1235,7 @@ uint8_t ValvePPInit(uint8_t index) {
|
||||
*/
|
||||
uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) {
|
||||
uint8_t rst = systemStatus.rst;
|
||||
SetValveRunMode(index, RTU_VALVE_CFG_MODE_PP);
|
||||
log_d("try to set angel to %d",angle);
|
||||
// 限制角度的逻辑不在这里，此处只执行控制逻辑
|
||||
if(angle > 360) {
|
||||
@ -1221,7 +1282,7 @@ void ReadValveSpeedPos(void)
|
||||
systemStatus.valvesSpeed[index] = ReadValveSpeed(index);
|
||||
systemStatus.valvesSpeedPercent[index] = transSpeedToSpeedPercent(index, abs(systemStatus.valvesSpeed[index]));
|
||||
systemStatus.valvesPos[index] = ReadValvePos(index);
|
||||
systemStatus.ds->valves.angle[index] = round(systemStatus.valvesPos[index]*360/dp.valve[index].fullCount);
|
||||
systemStatus.ds->valves.angle[index] = round((double)systemStatus.valvesPos[index] * 360.0 / dp.valve[index].fullCount);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1232,7 +1293,7 @@ void updateValveStatus(void)
|
||||
{
|
||||
valveCheckBTOResult(0);
|
||||
valveCheckBTOResult(1);
|
||||
if (systemStatus.ds->initStatus == INIT_IN_PROGRESS && systemStatus.isValveMovingBackToOrigin[0] == 0 && systemStatus.isValveMovingBackToOrigin[1] == 0) {
|
||||
if (systemStatus.isBTOOk[0] == 1 && systemStatus.isBTOOk[1] == 1) {
|
||||
systemStatus.ds->initStatus = INIT_SUCCESS;
|
||||
}
|
||||
|
||||
@ -1284,6 +1345,9 @@ void updateVPInfo(void)
|
||||
// ReadPumpSpeedPos();
|
||||
|
||||
// 获取阀门实时速度、位置
|
||||
// if(systemStatus.ds->initStatus == INIT_IN_PROGRESS) {
|
||||
// return; //初始化中不读取速度和位置
|
||||
// }
|
||||
ReadValveSpeedPos();
|
||||
|
||||
}
|
||||
|
||||
11
protocol.h
11
protocol.h
@ -18,7 +18,7 @@
|
||||
// 帧头帧尾定义
|
||||
#define FRAME_HEADER 0xA55A5AA5
|
||||
#define FRAME_TAIL 0x5AA5A55A
|
||||
#define READ_ACK_TIMEOUT 200
|
||||
#define READ_ACK_TIMEOUT 10
|
||||
#define ACK_OK 0x0000
|
||||
#define ACK_FAILED 0x0001
|
||||
#define ACK_OTHER 0x0002
|
||||
@ -370,9 +370,9 @@ typedef enum {
|
||||
|
||||
// 7. 初始化状态
|
||||
typedef enum {
|
||||
INIT_IN_PROGRESS = 0,
|
||||
INIT_SUCCESS = 1,
|
||||
INIT_FAILED = 2
|
||||
INIT_SUCCESS = 0,
|
||||
INIT_FAILED = 1,
|
||||
INIT_IN_PROGRESS = 2,
|
||||
} InitStatus_t;
|
||||
|
||||
// 大小对齐至1字节
|
||||
@ -411,6 +411,7 @@ typedef struct
|
||||
uint8_t pumpsSpeedPercent[2];//实时速度百分比
|
||||
uint32_t pumpsPos[2];//实时位置
|
||||
uint8_t isValveMovingBackToOrigin[2];//阀门是否在回归原点
|
||||
uint8_t isBTOOk[2];//回归原点是否完成
|
||||
uint16_t rst;//RTU命令执行结果
|
||||
|
||||
} SystemStatus_t;//包含需要上报的状态及附加状态
|
||||
@ -453,7 +454,7 @@ void runValveDemo(void);
|
||||
void updateSystemStatus(void);
|
||||
void initCTLSystem(void);
|
||||
void processReceivedData(uint8_t len);
|
||||
|
||||
void updateActivityMeter(float value);
|
||||
|
||||
|
||||
#endif // PROTOCOL_H
|
||||
Loading…
Reference in New Issue
Block a user