update functions and logic of checking ACK
This commit is contained in:
CSSC-WORK\murmur
parent
92485d58a9
commit
93c676e1cb
196
protocol.c
196
protocol.c
@ -1,6 +1,21 @@
|
||||
#include "protocol.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 = {
|
||||
.sensorStatus = 1,
|
||||
.valves = {210, 120},
|
||||
@ -11,20 +26,21 @@ DeviceStatus_t deviceStatus = {
|
||||
.initStatus = 1
|
||||
};
|
||||
SystemStatus_t systemStatus = {
|
||||
.speed = {0, 0},
|
||||
.pos = {0, 0}
|
||||
.speed = {0},
|
||||
.pos = {0},
|
||||
.rst = 0
|
||||
};
|
||||
|
||||
uint8_t isValveMovingBackToOrigin[2] = {0,0};
|
||||
|
||||
DeviceParam_t dp = {
|
||||
.pump = {
|
||||
{"pump1", 4, 60, 100, 100,40000},
|
||||
{"pump2", 3, 10, 100, 100,40000}
|
||||
{"pump1", 4, 60, 100, 100,40000,0},
|
||||
{"pump2", 3, 10, 100, 100,40000,0}
|
||||
},
|
||||
.valve = {
|
||||
{"valve1", 1, 10, 100, 100,47620},
|
||||
{"valve2", 2, 10, 100, 100,47620}
|
||||
{"valve1", 1, 10, 100, 100,47620,0},
|
||||
{"valve2", 2, 10, 100, 100,47620,0}
|
||||
}
|
||||
};
|
||||
|
||||
@ -180,36 +196,30 @@ void updateInitStatus(InitStatus_t status) {
|
||||
* ModBUS RTU写命令
|
||||
* 通过串口发送ModBUS RTU格式的命令
|
||||
*
|
||||
* 发送完成后回读反馈结果，累加到systemStatus.rst
|
||||
*
|
||||
* @param txBuf 发送数据缓冲区
|
||||
* @param txLen 发送数据长度
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
static uint8_t writeCMD(uint8_t *txBuf, uint16_t txLen) {
|
||||
printf("writeCMD: ");
|
||||
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);
|
||||
elog_hexdump("writeCMD", 16, txBuf, txLen);
|
||||
|
||||
transDataToMotorValve(txBuf, txLen);
|
||||
uint8_t rxBuf[30] = {0};
|
||||
uint16_t rxLen = txLen;
|
||||
uint8_t rst = HAL_UART_Receive(&huart2, rxBuf, rxLen, READ_ACK_TIMEOUT);
|
||||
if(memcmp(rxBuf, txBuf, 2) != 0) {
|
||||
uint8_t hal_rst = readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT);
|
||||
if(memcmp(rxBuf, txBuf, 2) != 0) {//正常情况下，返回的前2个字节应与发送的相同
|
||||
log_e("writeCMD error!");
|
||||
printf("%d readCMD: ", rst);
|
||||
for(uint16_t i = 0; i < rxLen; i++) {
|
||||
printf("%02X ", rxBuf[i]);
|
||||
}
|
||||
printf("\r\n");
|
||||
elog_hexdump("writeCMD error", 16, rxBuf, rxLen);
|
||||
systemStatus.rst += 1;//结果计数
|
||||
return 1;
|
||||
}
|
||||
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 txLen 发送数据长度
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
void sendMsgToHost(uint8_t *txBuf, uint16_t txLen) {
|
||||
// 发送数据
|
||||
// HAL_UART_Transmit_DMA(&huart1, txBuf, txLen);
|
||||
transDataToHost(txBuf, txLen);
|
||||
}
|
||||
|
||||
@ -562,7 +570,7 @@ uint16_t ReadPumpAlarm(uint8_t index) {
|
||||
}
|
||||
|
||||
/**
|
||||
* 解<EFBFBD><EFBFBD>泵的告警信息
|
||||
* 解码泵的告警信息
|
||||
*
|
||||
* @param reg4001 告警寄存器值
|
||||
*/
|
||||
@ -653,22 +661,33 @@ void UpdatePumpStatus() {
|
||||
*/
|
||||
uint8_t InitPump(void) {
|
||||
// 初始化泵
|
||||
log_e("InitPump");
|
||||
uint8_t rst = 0;
|
||||
rst |= SetPumpJogAcc(dp.pump[0].id, dp.pump[0].maxAccel);
|
||||
rst |= SetPumpJogDec(dp.pump[0].id, dp.pump[0].maxDecel);
|
||||
rst |= SetPumpJogSpeed(dp.pump[0].id, dp.pump[0].maxSpeed);
|
||||
rst |= SetPumpStepAcc(dp.pump[0].id, dp.pump[0].maxAccel);
|
||||
rst |= SetPumpStepDec(dp.pump[0].id, dp.pump[0].maxDecel);
|
||||
rst |= SetPumpStepSpeed(dp.pump[0].id, dp.pump[0].maxSpeed);
|
||||
log_i("InitPump");
|
||||
uint8_t rst = systemStatus.rst;
|
||||
|
||||
SetPumpJogAcc(0, dp.pump[0].maxAccel);
|
||||
SetPumpJogDec(0, dp.pump[0].maxDecel);
|
||||
SetPumpJogSpeed(0, dp.pump[0].maxSpeed);
|
||||
SetPumpStepAcc(0, dp.pump[0].maxAccel);
|
||||
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);
|
||||
DecodePumpAlarmMsg(reg4001);
|
||||
uint16_t reg4002 = ReadPumpStatus(0);
|
||||
@ -970,19 +989,21 @@ static uint8_t valveCheckBTOResult(uint8_t index, uint32_t startTime)
|
||||
* 阀门回归原点控制
|
||||
* 包含设置原点回归方式、堵转检测、运行模式等配置
|
||||
*
|
||||
* 回归需要时间，回归结果在轮询中检查
|
||||
*
|
||||
* @param index 阀门索引
|
||||
* @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.设置原点回归方式
|
||||
// (0416h)=37;17=负限位，18=正限位
|
||||
if(direction > 0) {
|
||||
printf("back to Origin, +\r\n");
|
||||
log_i("back to Origin, +\r\n");
|
||||
SetValveHomeDetectMode(index, 37);//正方向堵转
|
||||
}
|
||||
else {
|
||||
printf("back to Origin, -\r\n");
|
||||
log_i("back to Origin, -\r\n");
|
||||
SetValveHomeDetectMode(index, 38);//反方向堵转
|
||||
}
|
||||
// 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_RUN_ORIGIN);
|
||||
|
||||
if(rst != systemStatus.rst) {
|
||||
log_e("ValveBackToOrigin[%d] failed!",index);
|
||||
systemStatus.ds.initStatus = INIT_FAILED;
|
||||
}
|
||||
|
||||
// 2,3,4,5设置过后可不再设置
|
||||
// 1,6为必须
|
||||
// 堵转点即为原点，读位置应为0，或小于200
|
||||
@ -1018,7 +1044,8 @@ uint8_t ValveBackToOrigin(uint8_t index,int8_t direction) {
|
||||
* @return 0:成功 其他:失败
|
||||
*/
|
||||
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.配置模式：
|
||||
// 00B1h=0、运行模式 03C2h=0x01，使设备工作在轮廓位置模式；
|
||||
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_DISABLE);
|
||||
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:成功 其他:失败
|
||||
*/
|
||||
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) {
|
||||
log_e("阀门角度设置错误");
|
||||
@ -1052,17 +1086,22 @@ uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) {
|
||||
}
|
||||
|
||||
// 其它配置不变的情况下只需要写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 的上升沿接收新的位置命令，
|
||||
// 所以每次执行完一次运行后需 要把此位清零。)
|
||||
SetValveFunc(index, 0x2F);
|
||||
SetValveFunc(index, 0x3F);
|
||||
|
||||
if(rst != systemStatus.rst) {
|
||||
log_e("ValveRunToAngle[%d] failed!",index);
|
||||
systemStatus.ds.initStatus = INIT_FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 初始化阀门参数
|
||||
* 设置最大速度、加速度和减速度
|
||||
* 设置最大速度、加速度和减速度，默认模式为轮廓位置模式PP
|
||||
*
|
||||
* @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.initStatus = INIT_IN_PROGRESS;
|
||||
systemStatus.rst = 0;
|
||||
InitValve();
|
||||
InitPump();
|
||||
}
|
||||
|
||||
// 初始化处理
|
||||
static uint8_t HandleInit(void) {
|
||||
// 实现初始化逻辑
|
||||
// 1.更新状态为“初始化中”
|
||||
// 2.执行初始化流程，按协议要求执行，不含默认的初始化内容
|
||||
// 3.检查初始化结果，更新状态“成功”或“失败”
|
||||
InitDeviceStatus();
|
||||
return 1;
|
||||
// 2.执行默认的初始化内容，此步骤系统上电后会自动执行
|
||||
// 3.执行协议初始化流程
|
||||
// 4.检查初始化结果，更新状态“成功”或“失败”
|
||||
|
||||
// 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校验位+帧尾
|
||||
*
|
||||
* @param funcCode 功能码
|
||||
* @param isOK 执行结果
|
||||
* @param isOK 执行结果，1成功，0失败
|
||||
*/
|
||||
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)
|
||||
{
|
||||
printf("CMD_FRAME_HEADER_ERROR\r\n");
|
||||
log_e("CMD_FRAME_HEADER_ERROR\r\n");
|
||||
return CMD_FRAME_HEADER_ERROR;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
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)
|
||||
{
|
||||
printf("CMD_FRAME_CHECK_ERROR\r\n");
|
||||
log_e("CMD_FRAME_CHECK_ERROR\r\n");
|
||||
return CMD_FRAME_CHECK_ERROR;
|
||||
}
|
||||
return CMD_FRAME_OK;
|
||||
@ -1414,13 +1475,14 @@ CmdFrameError_t checkHostCmd(uint8_t *rxBuf, uint8_t rxLen) {
|
||||
* @param rxLen 接收到的数据长度
|
||||
* @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 (error != CMD_FRAME_OK)
|
||||
if (checkHostCmd(rxBuf, rxLen) != 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];//提取命令码
|
||||
uint8_t dataLen = rxBuf[sizeof(FRAME_HEADER)+2];//提取数据长度
|
||||
@ -1447,6 +1509,14 @@ CmdFrameError_t ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen) {
|
||||
break;
|
||||
case HOST_CMD_SYSTEM_INIT:
|
||||
error = HandleInit();
|
||||
if(error != 0) {
|
||||
log_e("系统初始化失败");
|
||||
packMsgToHost(HOST_CMD_SYSTEM_INIT, ACK_OK);
|
||||
}
|
||||
else {
|
||||
packMsgToHost(HOST_CMD_SYSTEM_INIT, ACK_FAILED);
|
||||
}
|
||||
|
||||
break;
|
||||
default:
|
||||
error = CMD_FRAME_CMD_ERROR;
|
||||
|
||||
19
protocol.h
19
protocol.h
@ -19,6 +19,9 @@
|
||||
#define FRAME_HEADER 0xA55A5AA5
|
||||
#define FRAME_TAIL 0x5AA5A55A
|
||||
#define READ_ACK_TIMEOUT 50
|
||||
#define ACK_OK 0x0001
|
||||
#define ACK_FAILED 0x0000
|
||||
#define ACK_OTHER 0x0002
|
||||
|
||||
// 功能码定义
|
||||
#define HOST_CMD_STATUS_QUERY 0x0001 // 状态查询
|
||||
@ -401,8 +404,9 @@ typedef struct {
|
||||
typedef struct
|
||||
{
|
||||
DeviceStatus_t ds;
|
||||
uint32_t speed[2];//实时速度
|
||||
uint32_t pos[2];//实时位置
|
||||
uint32_t speed[4];//实时速度
|
||||
uint32_t pos[4];//实时位置
|
||||
uint16_t rst;//RTU命令执行结果
|
||||
|
||||
} SystemStatus_t;
|
||||
|
||||
@ -414,10 +418,11 @@ typedef struct {
|
||||
uint32_t maxAccel;
|
||||
uint32_t maxDecel;
|
||||
uint16_t fullCount;//电机总步数，用于根据角度估算需要移动的步数
|
||||
int16_t offsetPos;//电机偏移位置，用于补偿电机移动误差
|
||||
} MotorDefaultParam_t;
|
||||
|
||||
|
||||
// 定义设备<EFBFBD><EFBFBD>认参数
|
||||
// 定义设备默认参数
|
||||
typedef struct {
|
||||
MotorDefaultParam_t pump[2];
|
||||
MotorDefaultParam_t valve[2];
|
||||
@ -428,17 +433,13 @@ extern DeviceParam_t dp;
|
||||
extern DeviceStatus_t deviceStatus;
|
||||
|
||||
// 函数声明
|
||||
CmdFrameError_t ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen);
|
||||
// uint16_t CalculateCRC16(uint8_t *data, uint16_t length);
|
||||
void InitDeviceStatus();
|
||||
void ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen);
|
||||
void DecodePumpAlarmMsg(uint16_t reg4001);
|
||||
void DecodePumpStatusMsg(uint16_t reg4002);
|
||||
void UpdatePumpStatus();
|
||||
void ProcessMotorMsg(uint8_t *rxBuf, uint16_t rxLen);
|
||||
void runPumpDemo(void);
|
||||
void runVavleDemo(void);
|
||||
void updateSystemStatus(void);
|
||||
void initSystem(void);
|
||||
void initCTLSystem(void);
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user