bug fix
This commit is contained in:
murmur
parent
1e6ba7eeea
commit
7e47936f43
117
protocol.c
117
protocol.c
@ -1,5 +1,6 @@
|
||||
#include "protocol.h"
|
||||
#include <string.h>
|
||||
#include "math.h"
|
||||
|
||||
#ifndef USE_ELOG
|
||||
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
|
||||
|
||||
const char *ackError[]={ "HAL_OK", "HAL_ERROR", "HAL_BUSY", "HAL_TIMEOUT" };
|
||||
|
||||
DeviceStatus_t deviceStatus = {
|
||||
.sensorStatus = 1,
|
||||
.valves = {{210, 120}},
|
||||
.pumps = {{0, 0},{50, 50}},
|
||||
.pumps = {{1, 2},{50, 70}},
|
||||
.bubbleStatus = 0,
|
||||
.activityMeter = 0,
|
||||
.activityMeter = 123.456,
|
||||
.estopStatus = 0,
|
||||
.errorCode = 0,
|
||||
.initStatus = 1
|
||||
};
|
||||
SystemStatus_t systemStatus = {
|
||||
.ds = &deviceStatus, // 初始化时将 ds 指向 deviceStatus
|
||||
.valvesSpeed = {0},
|
||||
.valvesPos = {0},
|
||||
.valvesSpeedPercent = {0},
|
||||
@ -203,11 +206,12 @@ static uint8_t writeCMD(uint8_t *txBuf, uint16_t txLen) {
|
||||
transDataToMotorValve(txBuf, txLen);
|
||||
uint8_t rxBuf[30] = {0};
|
||||
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);
|
||||
// elog_hexdump("ACK", 16, rxBuf, rxLen);
|
||||
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;//结果计数
|
||||
return 1;
|
||||
}
|
||||
@ -671,11 +675,11 @@ void ReadPumpSpeedPos(void)
|
||||
//判断正转、反转
|
||||
if(speed > 0) {
|
||||
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
|
||||
systemStatus.ds.pumps.status[index] = PUMP_STATUS_ANTICLOCKWISE;
|
||||
systemStatus.ds->pumps.status[index] = PUMP_STATUS_ANTICLOCKWISE;
|
||||
} 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) {
|
||||
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.设置原点回归方式
|
||||
// (0416h)=37;17=负限位，18=正限位
|
||||
if(direction > 0) {
|
||||
@ -1023,9 +1032,7 @@ void ValveBackToOrigin(uint8_t index,int8_t direction) {
|
||||
// (0170h)=300，(0172h)=50
|
||||
SetValveHomeTorque(index, 30);//30%
|
||||
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;
|
||||
// SetValveHomeSwtSpeed(index, 20000);
|
||||
SetValveHomeOriSpeed(index, 20000);
|
||||
@ -1040,7 +1047,7 @@ void ValveBackToOrigin(uint8_t index,int8_t direction) {
|
||||
|
||||
if(rst != systemStatus.rst) {
|
||||
log_e("ValveBackToOrigin[%d] CMD failed!",index);
|
||||
systemStatus.ds.initStatus = INIT_FAILED;
|
||||
systemStatus.ds->initStatus = INIT_FAILED;
|
||||
}
|
||||
systemStatus.isValveMovingBackToOrigin[index] = 1;
|
||||
|
||||
@ -1060,7 +1067,7 @@ 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) {//初始化中才检查
|
||||
if (systemStatus.ds->initStatus != INIT_IN_PROGRESS && systemStatus.isValveMovingBackToOrigin[index] == 1) {//初始化中才检查
|
||||
return;
|
||||
}
|
||||
|
||||
@ -1098,14 +1105,18 @@ static void valveCheckBTOResult(uint8_t index)
|
||||
// systemStatus.ds.initStatus = INIT_SUCCESS;
|
||||
systemStatus.isValveMovingBackToOrigin[index] = 0;
|
||||
log_i("ValveBackToOrigin[%d] success!",index);
|
||||
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
|
||||
SetValveRunMode(index, RTU_VALVE_CFG_MODE_PP);
|
||||
return;
|
||||
}
|
||||
else {
|
||||
log_e("ValveBackToOrigin[%d] failed!",index);
|
||||
retryCnt++;
|
||||
if(retryCnt > 2) {//执行两次回归，都失败则认为初始化失败
|
||||
systemStatus.ds.initStatus = INIT_FAILED;
|
||||
systemStatus.ds->initStatus = INIT_FAILED;
|
||||
retryCnt = 0;
|
||||
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
|
||||
SetValveRunMode(index, RTU_VALVE_CFG_MODE_PP);
|
||||
return;
|
||||
}
|
||||
// ValveBackToOrigin(index, -1);
|
||||
@ -1156,15 +1167,17 @@ uint8_t ValvePPInit(uint8_t index) {
|
||||
*/
|
||||
uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) {
|
||||
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) {
|
||||
log_e("阀门角度设置错误");
|
||||
log_e("阀门角度[%d]设置错误",angle);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// 其它配置不变的情况下只需要写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 的上升沿接收新的位置命令，
|
||||
// 所以每次执行完一次运行后需 要把此位清零。)
|
||||
@ -1172,9 +1185,10 @@ uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) {
|
||||
SetValveFunc(index, 0x3F);
|
||||
|
||||
if(rst != systemStatus.rst) {
|
||||
log_e("ValveRunToAngle[%d] failed!",index);
|
||||
log_e("ValveRunToAngle[%d] failed!",angle);
|
||||
// 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++) {
|
||||
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.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)
|
||||
{
|
||||
valveCheckBTOResult(0);
|
||||
// valveCheckBTOResult(1);
|
||||
if (systemStatus.ds.initStatus == INIT_IN_PROGRESS && systemStatus.isValveMovingBackToOrigin[0] == 0 && systemStatus.isValveMovingBackToOrigin[1] == 0) {
|
||||
systemStatus.ds.initStatus = INIT_SUCCESS;
|
||||
valveCheckBTOResult(1);
|
||||
if (systemStatus.ds->initStatus == INIT_IN_PROGRESS && systemStatus.isValveMovingBackToOrigin[0] == 0 && systemStatus.isValveMovingBackToOrigin[1] == 0) {
|
||||
systemStatus.ds->initStatus = INIT_SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
@ -1268,11 +1282,11 @@ void updateVPInfo(void)
|
||||
*/
|
||||
void dumpSystemStatus(void)
|
||||
{
|
||||
log_d("↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓");
|
||||
log_d("valve pos: %d[%d%%] / %d[%d%%]",systemStatus.valvesPos[0], \
|
||||
systemStatus.ds.valves.angle[0], \
|
||||
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]);
|
||||
systemStatus.ds->valves.angle[1]);
|
||||
log_d("valve speed: %d[%d%%] / %d[%d%%]",systemStatus.valvesSpeed[0], \
|
||||
systemStatus.valvesSpeedPercent[0], \
|
||||
systemStatus.valvesSpeed[1], \
|
||||
@ -1282,7 +1296,7 @@ void dumpSystemStatus(void)
|
||||
systemStatus.pumpsSpeedPercent[0], \
|
||||
systemStatus.pumpsSpeed[1], \
|
||||
systemStatus.pumpsSpeedPercent[1]);
|
||||
log_d("↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑");
|
||||
log_d("---------------");
|
||||
}
|
||||
|
||||
//在主循环中调用
|
||||
@ -1303,8 +1317,8 @@ void updateSystemStatus(void)
|
||||
*/
|
||||
void initCTLSystem(void)
|
||||
{
|
||||
systemStatus.ds = deviceStatus;
|
||||
systemStatus.ds.initStatus = INIT_IN_PROGRESS;
|
||||
systemStatus.ds = &deviceStatus; // 赋值地址而不是内容
|
||||
systemStatus.ds->initStatus = INIT_IN_PROGRESS;
|
||||
systemStatus.rst = 0;
|
||||
InitValve();
|
||||
// InitPump();
|
||||
@ -1330,15 +1344,30 @@ static void packMsgToHost(uint16_t funcCode, uint8_t isOK) {
|
||||
if(funcCode == HOST_CMD_STATUS_QUERY) {
|
||||
dlen = sizeof(DeviceStatus_t);
|
||||
index = sizeof(FRAME_TAIL)+2;
|
||||
msgBuf[index] = dlen;
|
||||
msgBuf[index] = 17;
|
||||
index += 1;
|
||||
memcpy(msgBuf+index, &deviceStatus, dlen);
|
||||
index += dlen;
|
||||
//系统是小端序，将deviceStatus转换为大端序
|
||||
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);//不包含帧头
|
||||
FillBigEndian16(msgBuf+index, crc);
|
||||
index += 2;
|
||||
FillBigEndian32(msgBuf+index, FRAME_HEADER);
|
||||
len = index+4;
|
||||
len = index+sizeof(FRAME_HEADER);
|
||||
}
|
||||
else {
|
||||
dlen = 1;
|
||||
@ -1351,7 +1380,7 @@ static void packMsgToHost(uint16_t funcCode, uint8_t isOK) {
|
||||
FillBigEndian16(msgBuf+index, crc);
|
||||
index += 2;
|
||||
FillBigEndian32(msgBuf+index, FRAME_HEADER);
|
||||
len = index+4;
|
||||
len = index+sizeof(FRAME_HEADER);
|
||||
}
|
||||
// 发送数据
|
||||
sendMsgToHost(msgBuf, len);
|
||||
@ -1365,9 +1394,9 @@ static uint8_t HandleInit(void) {
|
||||
// 3.执行协议初始化流程
|
||||
// 4.检查初始化结果，更新状态"成功"或"失败"
|
||||
|
||||
// 3.协议要求内容为：2个三通阀步进电机堵转找原点，重复至少2次，然后各自转至120°。
|
||||
// 3.协议要求内容为：2个三通阀步进电机堵转找原点，重复至少2次。
|
||||
|
||||
systemStatus.ds.initStatus = INIT_IN_PROGRESS;
|
||||
systemStatus.ds->initStatus = INIT_IN_PROGRESS;
|
||||
systemStatus.rst = 0;
|
||||
|
||||
initCTLSystem();
|
||||
@ -1405,12 +1434,12 @@ static void HandleStatusQuery(void) {
|
||||
static uint8_t HandleValveControl(uint8_t *Buff, uint8_t len) {
|
||||
// 实现三通阀控制逻辑
|
||||
|
||||
if(len != 8) {
|
||||
log_e("三通阀控制错误");
|
||||
if(len != 4) {
|
||||
log_e("三通阀控制错误[%d]",len);
|
||||
return 1;
|
||||
}
|
||||
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) {
|
||||
continue;
|
||||
@ -1440,7 +1469,7 @@ static uint8_t HandlePumpTimeControl(uint8_t *Buff, uint8_t len) {
|
||||
// 1表示启动泵顺时针转动，2表示启动泵的逆时针转动，0表示停止泵
|
||||
// 时间为0则表示一直转
|
||||
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) {
|
||||
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;
|
||||
@ -1527,7 +1556,7 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) {
|
||||
*/
|
||||
static uint8_t HandlePumpStepControl(uint8_t *Buff, uint8_t len) {
|
||||
// 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) {
|
||||
continue;
|
||||
@ -1561,7 +1590,7 @@ static uint8_t HandleSoftStop(uint8_t *rxBuf, uint16_t rxLen) {
|
||||
// 实现软急停功能逻辑
|
||||
if(rxBuf[0] == 0) {
|
||||
// 正常状态
|
||||
systemStatus.ds.estopStatus = ESTOP_NORMAL;
|
||||
systemStatus.ds->estopStatus = ESTOP_NORMAL;
|
||||
}
|
||||
else {
|
||||
// 急停状态
|
||||
@ -1576,7 +1605,7 @@ static uint8_t HandleSoftStop(uint8_t *rxBuf, uint16_t rxLen) {
|
||||
log_e("软急停错误");
|
||||
return ACK_FAILED;
|
||||
}
|
||||
systemStatus.ds.estopStatus = ESTOP_PRESSED;
|
||||
systemStatus.ds->estopStatus = ESTOP_PRESSED;
|
||||
}
|
||||
return ACK_OK;
|
||||
}
|
||||
|
||||
@ -401,8 +401,8 @@ typedef struct {
|
||||
|
||||
typedef struct
|
||||
{
|
||||
DeviceStatus_t ds;
|
||||
uint32_t valvesSpeed[2];//实时速度
|
||||
DeviceStatus_t *ds;
|
||||
int32_t valvesSpeed[2];//实时速度
|
||||
uint8_t valvesSpeedPercent[2];//实时速度百分比
|
||||
uint32_t valvesPos[2];//实时位置
|
||||
uint32_t pumpsSpeed[2];//实时速度
|
||||
|
||||
Loading…
Reference in New Issue
Block a user