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

bug fix

Browse Source
This commit is contained in:
murmur 2024-12-30 23:51:23 +08:00
parent 1e6ba7eeea
commit 7e47936f43
2 changed files with 75 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
protocol.c
View File

@ -1,5 +1,6 @@
#include "protocol.h" #include "protocol.h"
#include <string.h> #include <string.h>
#include "math.h"
#ifndef USE_ELOG #ifndef USE_ELOG
void elog_hexdump(const char *name, uint8_t width, const void *buf, uint16_t size) { void elog_hexdump(const char *name, uint8_t width, const void *buf, uint16_t size) {
@ -15,18 +16,20 @@ void elog_hexdump(const char *name, uint8_t width, const void *buf, uint16_t siz
} }
#endif #endif
const char *ackError[]={ "HAL_OK", "HAL_ERROR", "HAL_BUSY", "HAL_TIMEOUT" };
DeviceStatus_t deviceStatus = { DeviceStatus_t deviceStatus = {
.sensorStatus = 1, .sensorStatus = 1,
.valves = {{210, 120}}, .valves = {{210, 120}},
.pumps = {{0, 0},{50, 50}}, .pumps = {{1, 2},{50, 70}},
.bubbleStatus = 0, .bubbleStatus = 0,
.activityMeter = 0, .activityMeter = 123.456,
.estopStatus = 0, .estopStatus = 0,
.errorCode = 0, .errorCode = 0,
.initStatus = 1 .initStatus = 1
}; };
SystemStatus_t systemStatus = { SystemStatus_t systemStatus = {
.ds = &deviceStatus, // 初始化时将 ds 指向 deviceStatus
.valvesSpeed = {0}, .valvesSpeed = {0},
.valvesPos = {0}, .valvesPos = {0},
.valvesSpeedPercent = {0}, .valvesSpeedPercent = {0},
@ -203,11 +206,12 @@ static uint8_t writeCMD(uint8_t *txBuf, uint16_t txLen) {
transDataToMotorValve(txBuf, txLen); transDataToMotorValve(txBuf, 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); uint8_t ret = readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT);
// HAL_UART_Receive(&huart2, rxBuf, rxLen, READ_ACK_TIMEOUT*4); // HAL_UART_Receive(&huart2, rxBuf, rxLen, READ_ACK_TIMEOUT*4);
// elog_hexdump("ACK", 16, rxBuf, rxLen); // elog_hexdump("ACK", 16, rxBuf, rxLen);
if(memcmp(rxBuf, txBuf, 2) != 0) {//正常情况下，返回的前2个字节应与发送的相同 if(memcmp(rxBuf, txBuf, 2) != 0) {//正常情况下，返回的前2个字节应与发送的相同
elog_hexdump("writeCMD error", 16, rxBuf, rxLen); log_e("READ ACK ERROR:[%s]", ackError[ret]);
elog_hexdump("GET DATA", 16, rxBuf, rxLen);
systemStatus.rst += 1;//结果计数 systemStatus.rst += 1;//结果计数
return 1; return 1;
} }
@ -671,11 +675,11 @@ void ReadPumpSpeedPos(void)
//判断正转、反转 //判断正转、反转
if(speed > 0) { if(speed > 0) {
if(ReadPump2Reg(dp.pump[index].id,RTU_PUMP_CMD_POS) > pos) if(ReadPump2Reg(dp.pump[index].id,RTU_PUMP_CMD_POS) > pos)
systemStatus.ds.pumps.status[index] = PUMP_STATUS_CLOCKWISE; systemStatus.ds->pumps.status[index] = PUMP_STATUS_CLOCKWISE;
else else
systemStatus.ds.pumps.status[index] = PUMP_STATUS_ANTICLOCKWISE; systemStatus.ds->pumps.status[index] = PUMP_STATUS_ANTICLOCKWISE;
} else { } else {
systemStatus.ds.pumps.status[index] = PUMP_STATUS_STOP; systemStatus.ds->pumps.status[index] = PUMP_STATUS_STOP;
} }
} }
} }
@ -1009,6 +1013,11 @@ static uint32_t ReadValveSpeed(uint8_t index) {
*/ */
void ValveBackToOrigin(uint8_t index,int8_t direction) { void ValveBackToOrigin(uint8_t index,int8_t direction) {
uint8_t rst = systemStatus.rst; uint8_t rst = systemStatus.rst;
// 3.写 (00B1h)=0、运行模式 (03C2h)=0x06，使其工作在原点回归模式；
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
SetValveRunMode(index, RTU_VALVE_CFG_MODE_HM);
// 1.设置原点回归方式 // 1.设置原点回归方式
// (0416h)=37;17=负限位，18=正限位 // (0416h)=37;17=负限位，18=正限位
if(direction > 0) { if(direction > 0) {
@ -1023,9 +1032,7 @@ void ValveBackToOrigin(uint8_t index,int8_t direction) {
// (0170h)=300，(0172h)=50 // (0170h)=300，(0172h)=50
SetValveHomeTorque(index, 30);//30% SetValveHomeTorque(index, 30);//30%
SetValveHomeTime(index, 5);//5ms SetValveHomeTime(index, 5);//5ms
// 3.写 (00B1h)=0、运行模式 (03C2h)=0x06，使其工作在原点回归模式；
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
SetValveRunMode(index, RTU_VALVE_CFG_MODE_HM);
// 4.写寻找限位开关速度和寻找原点信号速度(0417h)= 10000 (0419h)=1000; // 4.写寻找限位开关速度和寻找原点信号速度(0417h)= 10000 (0419h)=1000;
// SetValveHomeSwtSpeed(index, 20000); // SetValveHomeSwtSpeed(index, 20000);
SetValveHomeOriSpeed(index, 20000); SetValveHomeOriSpeed(index, 20000);
@ -1040,7 +1047,7 @@ void ValveBackToOrigin(uint8_t index,int8_t direction) {
if(rst != systemStatus.rst) { if(rst != systemStatus.rst) {
log_e("ValveBackToOrigin[%d] CMD failed!",index); log_e("ValveBackToOrigin[%d] CMD failed!",index);
systemStatus.ds.initStatus = INIT_FAILED; systemStatus.ds->initStatus = INIT_FAILED;
} }
systemStatus.isValveMovingBackToOrigin[index] = 1; systemStatus.isValveMovingBackToOrigin[index] = 1;
@ -1060,7 +1067,7 @@ 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) {//初始化中才检查 if (systemStatus.ds->initStatus != INIT_IN_PROGRESS && systemStatus.isValveMovingBackToOrigin[index] == 1) {//初始化中才检查
return; return;
} }
@ -1098,14 +1105,18 @@ static void valveCheckBTOResult(uint8_t index)
// systemStatus.ds.initStatus = INIT_SUCCESS; // systemStatus.ds.initStatus = INIT_SUCCESS;
systemStatus.isValveMovingBackToOrigin[index] = 0; systemStatus.isValveMovingBackToOrigin[index] = 0;
log_i("ValveBackToOrigin[%d] success!",index); log_i("ValveBackToOrigin[%d] success!",index);
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
SetValveRunMode(index, RTU_VALVE_CFG_MODE_PP);
return; return;
} }
else { else {
log_e("ValveBackToOrigin[%d] failed!",index); 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;
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
SetValveRunMode(index, RTU_VALVE_CFG_MODE_PP);
return; return;
} }
// ValveBackToOrigin(index, -1); // ValveBackToOrigin(index, -1);
@ -1156,15 +1167,17 @@ uint8_t ValvePPInit(uint8_t index) {
*/ */
uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) { uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) {
uint8_t rst = systemStatus.rst; uint8_t rst = systemStatus.rst;
log_i("set angel to %d\r\n",angle); log_d("try to set angel to %d",angle);
// 限制角度的逻辑不在这里，此处只执行控制逻辑 // 限制角度的逻辑不在这里，此处只执行控制逻辑
if(angle > 360) { if(angle > 360) {
log_e("阀门角度设置错误"); log_e("阀门角度[%d]设置错误",angle);
return 1; return 1;
} }
// 其它配置不变的情况下只需要写3个控制字 // 其它配置不变的情况下只需要写3个控制字
SetValvePPPos(index, (uint32_t)(angle*dp.valve[index].fullCount/360+dp.valve[index].offsetPos)); uint32_t pos = (uint32_t)(angle*dp.valve[index].fullCount/360+dp.valve[index].offsetPos);
log_d("angle[%d] = pos[%d]",angle,pos);
SetValvePPPos(index, pos);
// 电机以绝对位置，立即更新的方式运行 // 电机以绝对位置，立即更新的方式运行
// (电机是以控制字 6040h(0380h)的 bit4 的上升沿接收新的位置命令， // (电机是以控制字 6040h(0380h)的 bit4 的上升沿接收新的位置命令，
// 所以每次执行完一次运行后需 要把此位清零。) // 所以每次执行完一次运行后需 要把此位清零。)
@ -1172,9 +1185,10 @@ uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) {
SetValveFunc(index, 0x3F); SetValveFunc(index, 0x3F);
if(rst != systemStatus.rst) { if(rst != systemStatus.rst) {
log_e("ValveRunToAngle[%d] failed!",index); log_e("ValveRunToAngle[%d] failed!",angle);
// systemStatus.ds.initStatus = INIT_FAILED; // systemStatus.ds.initStatus = INIT_FAILED;
} }
log_i("ValveRunToAngle[%d] success!",angle);
} }
/** /**
@ -1194,9 +1208,9 @@ 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, abs(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] = round(systemStatus.valvesPos[index]*360/dp.valve[index].fullCount);
} }
} }
@ -1206,9 +1220,9 @@ 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) { if (systemStatus.ds->initStatus == INIT_IN_PROGRESS && systemStatus.isValveMovingBackToOrigin[0] == 0 && systemStatus.isValveMovingBackToOrigin[1] == 0) {
systemStatus.ds.initStatus = INIT_SUCCESS; systemStatus.ds->initStatus = INIT_SUCCESS;
} }
@ -1268,11 +1282,11 @@ void updateVPInfo(void)
*/ */
void dumpSystemStatus(void) void dumpSystemStatus(void)
{ {
log_d("↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓"); log_d("---------------");
log_d("valve pos: %d[%d%%] / %d[%d%%]",systemStatus.valvesPos[0], \ log_d("valve pos: %d[%d°] / %d[%d°]",systemStatus.valvesPos[0], \
systemStatus.ds.valves.angle[0], \ systemStatus.ds->valves.angle[0], \
systemStatus.valvesPos[1], \ systemStatus.valvesPos[1], \
systemStatus.ds.valves.angle[1]); systemStatus.ds->valves.angle[1]);
log_d("valve speed: %d[%d%%] / %d[%d%%]",systemStatus.valvesSpeed[0], \ log_d("valve speed: %d[%d%%] / %d[%d%%]",systemStatus.valvesSpeed[0], \
systemStatus.valvesSpeedPercent[0], \ systemStatus.valvesSpeedPercent[0], \
systemStatus.valvesSpeed[1], \ systemStatus.valvesSpeed[1], \
@ -1282,7 +1296,7 @@ void dumpSystemStatus(void)
systemStatus.pumpsSpeedPercent[0], \ systemStatus.pumpsSpeedPercent[0], \
systemStatus.pumpsSpeed[1], \ systemStatus.pumpsSpeed[1], \
systemStatus.pumpsSpeedPercent[1]); systemStatus.pumpsSpeedPercent[1]);
log_d("↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑"); log_d("---------------");
} }
//在主循环中调用 //在主循环中调用
@ -1303,8 +1317,8 @@ void updateSystemStatus(void)
*/ */
void initCTLSystem(void) void initCTLSystem(void)
{ {
systemStatus.ds = deviceStatus; systemStatus.ds = &deviceStatus; // 赋值地址而不是内容
systemStatus.ds.initStatus = INIT_IN_PROGRESS; systemStatus.ds->initStatus = INIT_IN_PROGRESS;
systemStatus.rst = 0; systemStatus.rst = 0;
InitValve(); InitValve();
// InitPump(); // InitPump();
@ -1330,15 +1344,30 @@ static void packMsgToHost(uint16_t funcCode, uint8_t isOK) {
if(funcCode == HOST_CMD_STATUS_QUERY) { if(funcCode == HOST_CMD_STATUS_QUERY) {
dlen = sizeof(DeviceStatus_t); dlen = sizeof(DeviceStatus_t);
index = sizeof(FRAME_TAIL)+2; index = sizeof(FRAME_TAIL)+2;
msgBuf[index] = dlen; msgBuf[index] = 17;
index += 1; index += 1;
memcpy(msgBuf+index, &deviceStatus, dlen); //系统是小端序，将deviceStatus转换为大端序
index += dlen; memcpy(msgBuf+index, &deviceStatus.sensorStatus, 1);
index += 1;
FillBigEndian16(msgBuf+index, deviceStatus.valves.angle[0]);
index += 2;
FillBigEndian16(msgBuf+index, deviceStatus.valves.angle[1]);
index += 2;
memcpy(msgBuf+index, &deviceStatus.pumps, 5);
index += 5;
uint32_t hex_value;
memcpy(&hex_value, &deviceStatus.activityMeter, sizeof(deviceStatus.activityMeter));
FillBigEndian32(msgBuf+index, hex_value);
index += 4;
memcpy(msgBuf+index, &deviceStatus.estopStatus, 3);
index += 3;
uint16_t crc = CalculateCRC16(msgBuf+4, index-4);//不包含帧头 uint16_t crc = CalculateCRC16(msgBuf+4, index-4);//不包含帧头
FillBigEndian16(msgBuf+index, crc); FillBigEndian16(msgBuf+index, crc);
index += 2; index += 2;
FillBigEndian32(msgBuf+index, FRAME_HEADER); FillBigEndian32(msgBuf+index, FRAME_HEADER);
len = index+4; len = index+sizeof(FRAME_HEADER);
} }
else { else {
dlen = 1; dlen = 1;
@ -1351,7 +1380,7 @@ static void packMsgToHost(uint16_t funcCode, uint8_t isOK) {
FillBigEndian16(msgBuf+index, crc); FillBigEndian16(msgBuf+index, crc);
index += 2; index += 2;
FillBigEndian32(msgBuf+index, FRAME_HEADER); FillBigEndian32(msgBuf+index, FRAME_HEADER);
len = index+4; len = index+sizeof(FRAME_HEADER);
} }
// 发送数据 // 发送数据
sendMsgToHost(msgBuf, len); sendMsgToHost(msgBuf, len);
@ -1365,9 +1394,9 @@ static uint8_t HandleInit(void) {
// 3.执行协议初始化流程 // 3.执行协议初始化流程
// 4.检查初始化结果，更新状态"成功"或"失败" // 4.检查初始化结果，更新状态"成功"或"失败"
// 3.协议要求内容为：2个三通阀步进电机堵转找原点，重复至少2次，然后各自转至120°。 // 3.协议要求内容为：2个三通阀步进电机堵转找原点，重复至少2次。
systemStatus.ds.initStatus = INIT_IN_PROGRESS; systemStatus.ds->initStatus = INIT_IN_PROGRESS;
systemStatus.rst = 0; systemStatus.rst = 0;
initCTLSystem(); initCTLSystem();
@ -1405,12 +1434,12 @@ static void HandleStatusQuery(void) {
static uint8_t HandleValveControl(uint8_t *Buff, uint8_t len) { static uint8_t HandleValveControl(uint8_t *Buff, uint8_t len) {
// 实现三通阀控制逻辑 // 实现三通阀控制逻辑
if(len != 8) { if(len != 4) {
log_e("三通阀控制错误"); log_e("三通阀控制错误[%d]",len);
return 1; return 1;
} }
uint8_t rst = systemStatus.rst; uint8_t rst = systemStatus.rst;
for(uint8_t index = 0; index < len; index++) { for(uint8_t index = 0; index < len/2; index++) {
if(memcmp(Buff+index*2, "\xFF\xFF", 2) == 0) { if(memcmp(Buff+index*2, "\xFF\xFF", 2) == 0) {
continue; continue;
@ -1440,7 +1469,7 @@ static uint8_t HandlePumpTimeControl(uint8_t *Buff, uint8_t len) {
// 1表示启动泵顺时针转动，2表示启动泵的逆时针转动，0表示停止泵 // 1表示启动泵顺时针转动，2表示启动泵的逆时针转动，0表示停止泵
// 时间为0则表示一直转 // 时间为0则表示一直转
uint8_t rst = systemStatus.rst; uint8_t rst = systemStatus.rst;
for(uint8_t index = 0; index < len; index++) { for(uint8_t index = 0; index < len/3; index++) {
if(memcmp(Buff+index*3, "\xFF\xFF\xFF", 3) == 0) { if(memcmp(Buff+index*3, "\xFF\xFF\xFF", 3) == 0) {
continue; continue;
@ -1511,7 +1540,7 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) {
} }
//更新参数 //更新参数
systemStatus.ds.pumps.speed[index] = speedPercent; systemStatus.ds->pumps.speed[index] = speedPercent;
} }
return ACK_OK; return ACK_OK;
@ -1527,7 +1556,7 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) {
*/ */
static uint8_t HandlePumpStepControl(uint8_t *Buff, uint8_t len) { static uint8_t HandlePumpStepControl(uint8_t *Buff, uint8_t len) {
// 4字节步进，全FF跳过 // 4字节步进，全FF跳过
for (size_t index = 0; index < len; index++) for (size_t index = 0; index < len/4; index++)
{ {
if(memcmp(Buff+index*4, "\xFF\xFF\xFF\xFF", 4) == 0) { if(memcmp(Buff+index*4, "\xFF\xFF\xFF\xFF", 4) == 0) {
continue; continue;
@ -1561,7 +1590,7 @@ static uint8_t HandleSoftStop(uint8_t *rxBuf, uint16_t rxLen) {
// 实现软急停功能逻辑 // 实现软急停功能逻辑
if(rxBuf[0] == 0) { if(rxBuf[0] == 0) {
// 正常状态 // 正常状态
systemStatus.ds.estopStatus = ESTOP_NORMAL; systemStatus.ds->estopStatus = ESTOP_NORMAL;
} }
else { else {
// 急停状态 // 急停状态
@ -1576,7 +1605,7 @@ static uint8_t HandleSoftStop(uint8_t *rxBuf, uint16_t rxLen) {
log_e("软急停错误"); log_e("软急停错误");
return ACK_FAILED; return ACK_FAILED;
} }
systemStatus.ds.estopStatus = ESTOP_PRESSED; systemStatus.ds->estopStatus = ESTOP_PRESSED;
} }
return ACK_OK; return ACK_OK;
} }

4
protocol.h
View File

@ -401,8 +401,8 @@ typedef struct {
typedef struct typedef struct
{ {
DeviceStatus_t ds; DeviceStatus_t *ds;
uint32_t valvesSpeed[2];//实时速度 int32_t valvesSpeed[2];//实时速度
uint8_t valvesSpeedPercent[2];//实时速度百分比 uint8_t valvesSpeedPercent[2];//实时速度百分比
uint32_t valvesPos[2];//实时位置 uint32_t valvesPos[2];//实时位置
uint32_t pumpsSpeed[2];//实时速度 uint32_t pumpsSpeed[2];//实时速度