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

update functions and logic of checking ACK

Browse Source
This commit is contained in:
CSSC-WORK\murmur 2024-12-26 14:25:41 +08:00
parent 92485d58a9
commit 93c676e1cb
2 changed files with 143 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

196
protocol.c
View File

@ -1,6 +1,21 @@
#include "protocol.h" #include "protocol.h"
#include <string.h> #include <string.h>
#ifndef USE_ELOG
void elog_hexdump(const char *name, uint8_t width, const void *buf, uint16_t size) {
printf("%s: ", name);
// 按width的宽度打印
for(uint16_t i = 0; i < size; i++) {
printf("%02X ", ((uint8_t*)buf)[i]);
if((i+1)%width == 0) {
printf("\r\n");
}
}
printf("\r\n");
}
#endif
DeviceStatus_t deviceStatus = { DeviceStatus_t deviceStatus = {
.sensorStatus = 1, .sensorStatus = 1,
.valves = {210, 120}, .valves = {210, 120},
@ -11,20 +26,21 @@ DeviceStatus_t deviceStatus = {
.initStatus = 1 .initStatus = 1
}; };
SystemStatus_t systemStatus = { SystemStatus_t systemStatus = {
.speed = {0, 0}, .speed = {0},
.pos = {0, 0} .pos = {0},
.rst = 0
}; };
uint8_t isValveMovingBackToOrigin[2] = {0,0}; uint8_t isValveMovingBackToOrigin[2] = {0,0};
DeviceParam_t dp = { DeviceParam_t dp = {
.pump = { .pump = {
{"pump1", 4, 60, 100, 100,40000}, {"pump1", 4, 60, 100, 100,40000,0},
{"pump2", 3, 10, 100, 100,40000} {"pump2", 3, 10, 100, 100,40000,0}
}, },
.valve = { .valve = {
{"valve1", 1, 10, 100, 100,47620}, {"valve1", 1, 10, 100, 100,47620,0},
{"valve2", 2, 10, 100, 100,47620} {"valve2", 2, 10, 100, 100,47620,0}
} }
}; };
@ -180,36 +196,30 @@ void updateInitStatus(InitStatus_t status) {
* ModBUS RTU写命令 * ModBUS RTU写命令
* 通过串口发送ModBUS RTU格式的命令 * 通过串口发送ModBUS RTU格式的命令
* *
* 发送完成后回读反馈结果，累加到systemStatus.rst
*
* @param txBuf 发送数据缓冲区 * @param txBuf 发送数据缓冲区
* @param txLen 发送数据长度 * @param txLen 发送数据长度
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t writeCMD(uint8_t *txBuf, uint16_t txLen) { static uint8_t writeCMD(uint8_t *txBuf, uint16_t txLen) {
printf("writeCMD: "); elog_hexdump("writeCMD", 16, txBuf, txLen);
for(uint16_t i = 0; i < txLen; i++) {
printf("%02X ", txBuf[i]);
}
printf("\r\n");
// HAL_GPIO_WritePin(DIR2_GPIO_Port, DIR2_Pin, GPIO_PIN_SET);
// HAL_UART_Transmit(&huart2, txBuf, txLen, 100);
// HAL_GPIO_WritePin(DIR2_GPIO_Port, DIR2_Pin, GPIO_PIN_RESET);
transDataToMotorValve(txBuf, txLen); transDataToMotorValve(txBuf, txLen);
uint8_t rxBuf[30] = {0}; uint8_t rxBuf[30] = {0};
uint16_t rxLen = txLen; uint16_t rxLen = txLen;
uint8_t rst = HAL_UART_Receive(&huart2, rxBuf, rxLen, READ_ACK_TIMEOUT); uint8_t hal_rst = readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT);
if(memcmp(rxBuf, txBuf, 2) != 0) { if(memcmp(rxBuf, txBuf, 2) != 0) {//正常情况下，返回的前2个字节应与发送的相同
log_e("writeCMD error!"); log_e("writeCMD error!");
printf("%d readCMD: ", rst); elog_hexdump("writeCMD error", 16, rxBuf, rxLen);
for(uint16_t i = 0; i < rxLen; i++) { systemStatus.rst += 1;//结果计数
printf("%02X ", rxBuf[i]);
}
printf("\r\n");
return 1; return 1;
} }
else { else {
log_d("writeCMD success!"); log_i("writeCMD success!");
systemStatus.rst += 0;//结果计数
return 0;
} }
return 0;
} }
/** /**
@ -217,11 +227,9 @@ static uint8_t writeCMD(uint8_t *txBuf, uint16_t txLen) {
* *
* @param txBuf 发送数据缓冲区 * @param txBuf 发送数据缓冲区
* @param txLen 发送数据长度 * @param txLen 发送数据长度
* @return 0:成功 其他:失败
*/ */
void sendMsgToHost(uint8_t *txBuf, uint16_t txLen) { void sendMsgToHost(uint8_t *txBuf, uint16_t txLen) {
// 发送数据 // 发送数据
// HAL_UART_Transmit_DMA(&huart1, txBuf, txLen);
transDataToHost(txBuf, txLen); transDataToHost(txBuf, txLen);
} }
@ -562,7 +570,7 @@ uint16_t ReadPumpAlarm(uint8_t index) {
} }
/** /**
* 解<EFBFBD><EFBFBD>泵的告警信息 * 解码泵的告警信息
* *
* @param reg4001 告警寄存器值 * @param reg4001 告警寄存器值
*/ */
@ -653,22 +661,33 @@ void UpdatePumpStatus() {
*/ */
uint8_t InitPump(void) { uint8_t InitPump(void) {
// 初始化泵 // 初始化泵
log_e("InitPump"); log_i("InitPump");
uint8_t rst = 0; uint8_t rst = systemStatus.rst;
rst |= SetPumpJogAcc(dp.pump[0].id, dp.pump[0].maxAccel);
rst |= SetPumpJogDec(dp.pump[0].id, dp.pump[0].maxDecel); SetPumpJogAcc(0, dp.pump[0].maxAccel);
rst |= SetPumpJogSpeed(dp.pump[0].id, dp.pump[0].maxSpeed); SetPumpJogDec(0, dp.pump[0].maxDecel);
rst |= SetPumpStepAcc(dp.pump[0].id, dp.pump[0].maxAccel); SetPumpJogSpeed(0, dp.pump[0].maxSpeed);
rst |= SetPumpStepDec(dp.pump[0].id, dp.pump[0].maxDecel); SetPumpStepAcc(0, dp.pump[0].maxAccel);
rst |= SetPumpStepSpeed(dp.pump[0].id, dp.pump[0].maxSpeed); SetPumpStepDec(0, dp.pump[0].maxDecel);
SetPumpStepSpeed(0, dp.pump[0].maxSpeed);
if (rst != systemStatus.rst)
{
log_e("InitPump[0] failed!");
systemStatus.ds.initStatus = INIT_FAILED;
}
SetPumpJogAcc(1, dp.pump[1].maxAccel);
SetPumpJogDec(1, dp.pump[1].maxDecel);
SetPumpJogSpeed(1, dp.pump[1].maxSpeed);
SetPumpStepAcc(1, dp.pump[1].maxAccel);
SetPumpStepDec(1, dp.pump[1].maxDecel);
SetPumpStepSpeed(1, dp.pump[1].maxSpeed);
if (rst != systemStatus.rst)
{
log_e("InitPump[1] failed!");
systemStatus.ds.initStatus = INIT_FAILED;
}
rst |= SetPumpJogAcc(dp.pump[1].id, dp.pump[1].maxAccel);
rst |= SetPumpJogDec(dp.pump[1].id, dp.pump[1].maxDecel);
rst |= SetPumpJogSpeed(dp.pump[1].id, dp.pump[1].maxSpeed);
rst |= SetPumpStepAcc(dp.pump[1].id, dp.pump[1].maxAccel);
rst |= SetPumpStepDec(dp.pump[1].id, dp.pump[1].maxDecel);
rst |= SetPumpStepSpeed(dp.pump[1].id, dp.pump[1].maxSpeed);
uint16_t reg4001 = ReadPumpAlarm(0); uint16_t reg4001 = ReadPumpAlarm(0);
DecodePumpAlarmMsg(reg4001); DecodePumpAlarmMsg(reg4001);
uint16_t reg4002 = ReadPumpStatus(0); uint16_t reg4002 = ReadPumpStatus(0);
@ -970,19 +989,21 @@ static uint8_t valveCheckBTOResult(uint8_t index, uint32_t startTime)
* 阀门回归原点控制 * 阀门回归原点控制
* 包含设置原点回归方式、堵转检测、运行模式等配置 * 包含设置原点回归方式、堵转检测、运行模式等配置
* *
* 回归需要时间，回归结果在轮询中检查
*
* @param index 阀门索引 * @param index 阀门索引
* @param direction 方向，正数正方向堵转，负数反方向堵转 * @param direction 方向，正数正方向堵转，负数反方向堵转
* @return 0:成功 其他:失败
*/ */
uint8_t ValveBackToOrigin(uint8_t index,int8_t direction) { void ValveBackToOrigin(uint8_t index,int8_t direction) {
uint8_t rst = systemStatus.rst;
// 1.设置原点回归方式 // 1.设置原点回归方式
// (0416h)=37;17=负限位，18=正限位 // (0416h)=37;17=负限位，18=正限位
if(direction > 0) { if(direction > 0) {
printf("back to Origin, +\r\n"); log_i("back to Origin, +\r\n");
SetValveHomeDetectMode(index, 37);//正方向堵转 SetValveHomeDetectMode(index, 37);//正方向堵转
} }
else { else {
printf("back to Origin, -\r\n"); log_i("back to Origin, -\r\n");
SetValveHomeDetectMode(index, 38);//反方向堵转 SetValveHomeDetectMode(index, 38);//反方向堵转
} }
// 2.设置堵转检测力矩和堵转检测时间 // 2.设置堵转检测力矩和堵转检测时间
@ -1004,6 +1025,11 @@ uint8_t ValveBackToOrigin(uint8_t index,int8_t direction) {
SetValveFunc(index, RTU_VALVE_CFG_ENABLE); SetValveFunc(index, RTU_VALVE_CFG_ENABLE);
SetValveFunc(index, RTU_VALVE_CFG_RUN_ORIGIN); SetValveFunc(index, RTU_VALVE_CFG_RUN_ORIGIN);
if(rst != systemStatus.rst) {
log_e("ValveBackToOrigin[%d] failed!",index);
systemStatus.ds.initStatus = INIT_FAILED;
}
// 2,3,4,5设置过后可不再设置 // 2,3,4,5设置过后可不再设置
// 1,6为必须 // 1,6为必须
// 堵转点即为原点，读位置应为0，或小于200 // 堵转点即为原点，读位置应为0，或小于200
@ -1018,7 +1044,8 @@ uint8_t ValveBackToOrigin(uint8_t index,int8_t direction) {
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
uint8_t ValvePPInit(uint8_t index) { uint8_t ValvePPInit(uint8_t index) {
printf("set mode to PP\r\n"); uint8_t rst = systemStatus.rst;
log_i("set mode to PP\r\n");
// 1.配置模式： // 1.配置模式：
// 00B1h=0、运行模式 03C2h=0x01，使设备工作在轮廓位置模式； // 00B1h=0、运行模式 03C2h=0x01，使设备工作在轮廓位置模式；
SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402); SetValveCOMMMode(index, RTU_VALVE_CFG_COMM_CIA402);
@ -1034,6 +1061,12 @@ uint8_t ValvePPInit(uint8_t index) {
SetValveFunc(index, RTU_VALVE_CFG_PREPARE); SetValveFunc(index, RTU_VALVE_CFG_PREPARE);
SetValveFunc(index, RTU_VALVE_CFG_DISABLE); SetValveFunc(index, RTU_VALVE_CFG_DISABLE);
SetValveFunc(index, RTU_VALVE_CFG_ENABLE); SetValveFunc(index, RTU_VALVE_CFG_ENABLE);
if (rst != systemStatus.rst)
{
log_e("ValvePPInit[%d] failed!",index);
systemStatus.ds.initStatus = INIT_FAILED;
}
} }
/** /**
@ -1044,7 +1077,8 @@ uint8_t ValvePPInit(uint8_t index) {
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) { uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) {
printf("set angel to %d\r\n",angle); uint8_t rst = systemStatus.rst;
log_i("set angel to %d\r\n",angle);
// 限制角度的逻辑不在这里，此处只执行控制逻辑 // 限制角度的逻辑不在这里，此处只执行控制逻辑
if(angle > 360) { if(angle > 360) {
log_e("阀门角度设置错误"); log_e("阀门角度设置错误");
@ -1052,17 +1086,22 @@ uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) {
} }
// 其它配置不变的情况下只需要写3个控制字 // 其它配置不变的情况下只需要写3个控制字
SetValvePPPos(index, (uint32_t)(angle*dp.valve[index].fullCount/360)); SetValvePPPos(index, (uint32_t)(angle*dp.valve[index].fullCount/360+dp.valve[index].offsetPos));
// 电机以绝对位置，立即更新的方式运行 // 电机以绝对位置，立即更新的方式运行
// (电机是以控制字 6040h(0380h)的 bit4 的上升沿接收新的位置命令， // (电机是以控制字 6040h(0380h)的 bit4 的上升沿接收新的位置命令，
// 所以每次执行完一次运行后需 要把此位清零。) // 所以每次执行完一次运行后需 要把此位清零。)
SetValveFunc(index, 0x2F); SetValveFunc(index, 0x2F);
SetValveFunc(index, 0x3F); SetValveFunc(index, 0x3F);
if(rst != systemStatus.rst) {
log_e("ValveRunToAngle[%d] failed!",index);
systemStatus.ds.initStatus = INIT_FAILED;
}
} }
/** /**
* 初始化阀门参数 * 初始化阀门参数
* 设置最大速度、加速度和减速度 * 设置最大速度、加速度和减速度，默认模式为轮廓位置模式PP
* *
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
@ -1154,21 +1193,43 @@ void updateSystemStatus(void)
} }
/** /**
* 初始化系统 * 初始化系统，初始化阀门和泵的默认参数，
* 有别于 HOST_CMD_SYSTEM_INIT=0x0007 指令对应的初始化功能
*/ */
void initSystem(void) void initCTLSystem(void)
{ {
systemStatus.ds = deviceStatus; systemStatus.ds = deviceStatus;
systemStatus.ds.initStatus = INIT_IN_PROGRESS;
systemStatus.rst = 0;
InitValve(); InitValve();
InitPump();
} }
// 初始化处理 // 初始化处理
static uint8_t HandleInit(void) { static uint8_t HandleInit(void) {
// 实现初始化逻辑 // 实现初始化逻辑
// 1.更新状态为“初始化中” // 1.更新状态为“初始化中”
// 2.执行初始化流程，按协议要求执行，不含默认的初始化内容 // 2.执行默认的初始化内容，此步骤系统上电后会自动执行
// 3.检查初始化结果，更新状态“成功”或“失败” // 3.执行协议初始化流程
InitDeviceStatus(); // 4.检查初始化结果，更新状态“成功”或“失败”
return 1;
// 3.协议要求内容为：2个三通阀步进电机堵转找原点，重复至少2次，然后各自转至120°。
systemStatus.ds.initStatus = INIT_IN_PROGRESS;
systemStatus.rst = 0;
initCTLSystem();
ValveBackToOrigin(0,-1);
ValveRunToAngle(0,60);
ValveBackToOrigin(0,-1);
ValveRunToAngle(0,120);
ValveBackToOrigin(1,-1);
ValveRunToAngle(1,60);
ValveBackToOrigin(1,-1);
ValveRunToAngle(1,120);
return systemStatus.rst;
} }
@ -1328,7 +1389,7 @@ static uint8_t HandleSoftStop(uint8_t *rxBuf, uint16_t rxLen) {
* 帧格式:帧头+功能码(2Byte)+数据长度(1Byte)+具体数据(NByte)+CRC16校验位+帧尾 * 帧格式:帧头+功能码(2Byte)+数据长度(1Byte)+具体数据(NByte)+CRC16校验位+帧尾
* *
* @param funcCode 功能码 * @param funcCode 功能码
* @param isOK 执行结果 * @param isOK 执行结果，1成功，0失败
*/ */
void packMsgToHost(uint16_t funcCode, uint8_t isOK) { void packMsgToHost(uint16_t funcCode, uint8_t isOK) {
// 实现打包消息到上位机逻辑 // 实现打包消息到上位机逻辑
@ -1387,19 +1448,19 @@ CmdFrameError_t checkHostCmd(uint8_t *rxBuf, uint8_t rxLen) {
if(memcmp(rxBuf, header, sizeof(FRAME_HEADER)) != 0) if(memcmp(rxBuf, header, sizeof(FRAME_HEADER)) != 0)
{ {
printf("CMD_FRAME_HEADER_ERROR\r\n"); log_e("CMD_FRAME_HEADER_ERROR\r\n");
return CMD_FRAME_HEADER_ERROR; return CMD_FRAME_HEADER_ERROR;
} }
if (memcmp(rxBuf + rxLen - sizeof(FRAME_TAIL), tail, sizeof(FRAME_TAIL)) != 0) if (memcmp(rxBuf + rxLen - sizeof(FRAME_TAIL), tail, sizeof(FRAME_TAIL)) != 0)
{ {
printf("CMD_FRAME_TAIL_ERROR\r\n"); log_e("CMD_FRAME_TAIL_ERROR\r\n");
return CMD_FRAME_TAIL_ERROR; return CMD_FRAME_TAIL_ERROR;
} }
uint16_t crc = CalculateCRC16(rxBuf+sizeof(FRAME_HEADER), rxLen - sizeof(FRAME_HEADER)-sizeof(FRAME_TAIL)-2); // 计算crc，不包含帧头和帧尾和crc自身 uint16_t crc = CalculateCRC16(rxBuf+sizeof(FRAME_HEADER), rxLen - sizeof(FRAME_HEADER)-sizeof(FRAME_TAIL)-2); // 计算crc，不包含帧头和帧尾和crc自身
if (((rxBuf[rxLen-sizeof(FRAME_TAIL)-2]<<8) | rxBuf[rxLen-sizeof(FRAME_TAIL)-1]) != crc) if (((rxBuf[rxLen-sizeof(FRAME_TAIL)-2]<<8) | rxBuf[rxLen-sizeof(FRAME_TAIL)-1]) != crc)
{ {
printf("CMD_FRAME_CHECK_ERROR\r\n"); log_e("CMD_FRAME_CHECK_ERROR\r\n");
return CMD_FRAME_CHECK_ERROR; return CMD_FRAME_CHECK_ERROR;
} }
return CMD_FRAME_OK; return CMD_FRAME_OK;
@ -1414,13 +1475,14 @@ CmdFrameError_t checkHostCmd(uint8_t *rxBuf, uint8_t rxLen) {
* @param rxLen 接收到的数据长度 * @param rxLen 接收到的数据长度
* @return 命令帧错误码 * @return 命令帧错误码
*/ */
CmdFrameError_t ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen) { void ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen) {
CmdFrameError_t error = checkHostCmd(rxBuf, rxLen); if (checkHostCmd(rxBuf, rxLen) != CMD_FRAME_OK)
if (error != CMD_FRAME_OK)
{ {
return error; log_e("命令错误");
return;
} }
uint8_t error = 0;
uint16_t cmdCode = (rxBuf[sizeof(FRAME_HEADER)] << 8) | rxBuf[sizeof(FRAME_HEADER)+1];//提取命令码 uint16_t cmdCode = (rxBuf[sizeof(FRAME_HEADER)] << 8) | rxBuf[sizeof(FRAME_HEADER)+1];//提取命令码
uint8_t dataLen = rxBuf[sizeof(FRAME_HEADER)+2];//提取数据长度 uint8_t dataLen = rxBuf[sizeof(FRAME_HEADER)+2];//提取数据长度
@ -1447,6 +1509,14 @@ CmdFrameError_t ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen) {
break; break;
case HOST_CMD_SYSTEM_INIT: case HOST_CMD_SYSTEM_INIT:
error = HandleInit(); error = HandleInit();
if(error != 0) {
log_e("系统初始化失败");
packMsgToHost(HOST_CMD_SYSTEM_INIT, ACK_OK);
}
else {
packMsgToHost(HOST_CMD_SYSTEM_INIT, ACK_FAILED);
}
break; break;
default: default:
error = CMD_FRAME_CMD_ERROR; error = CMD_FRAME_CMD_ERROR;

19
protocol.h
View File

@ -19,6 +19,9 @@
#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 50
#define ACK_OK 0x0001
#define ACK_FAILED 0x0000
#define ACK_OTHER 0x0002
// 功能码定义 // 功能码定义
#define HOST_CMD_STATUS_QUERY 0x0001 // 状态查询 #define HOST_CMD_STATUS_QUERY 0x0001 // 状态查询
@ -401,8 +404,9 @@ typedef struct {
typedef struct typedef struct
{ {
DeviceStatus_t ds; DeviceStatus_t ds;
uint32_t speed[2];//实时速度 uint32_t speed[4];//实时速度
uint32_t pos[2];//实时位置 uint32_t pos[4];//实时位置
uint16_t rst;//RTU命令执行结果
} SystemStatus_t; } SystemStatus_t;
@ -414,10 +418,11 @@ typedef struct {
uint32_t maxAccel; uint32_t maxAccel;
uint32_t maxDecel; uint32_t maxDecel;
uint16_t fullCount;//电机总步数，用于根据角度估算需要移动的步数 uint16_t fullCount;//电机总步数，用于根据角度估算需要移动的步数
int16_t offsetPos;//电机偏移位置，用于补偿电机移动误差
} MotorDefaultParam_t; } MotorDefaultParam_t;
// 定义设备<EFBFBD><EFBFBD>认参数 // 定义设备默认参数
typedef struct { typedef struct {
MotorDefaultParam_t pump[2]; MotorDefaultParam_t pump[2];
MotorDefaultParam_t valve[2]; MotorDefaultParam_t valve[2];
@ -428,17 +433,13 @@ extern DeviceParam_t dp;
extern DeviceStatus_t deviceStatus; extern DeviceStatus_t deviceStatus;
// 函数声明 // 函数声明
CmdFrameError_t ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen); void ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen);
// uint16_t CalculateCRC16(uint8_t *data, uint16_t length);
void InitDeviceStatus();
void DecodePumpAlarmMsg(uint16_t reg4001); void DecodePumpAlarmMsg(uint16_t reg4001);
void DecodePumpStatusMsg(uint16_t reg4002); void DecodePumpStatusMsg(uint16_t reg4002);
void UpdatePumpStatus();
void ProcessMotorMsg(uint8_t *rxBuf, uint16_t rxLen);
void runPumpDemo(void); void runPumpDemo(void);
void runVavleDemo(void); void runVavleDemo(void);
void updateSystemStatus(void); void updateSystemStatus(void);
void initSystem(void); void initCTLSystem(void);