From 93c676e1cbaa7e278f9ed81646067c4fda37e5a1 Mon Sep 17 00:00:00 2001 From: "CSSC-WORK\\murmur" Date: Thu, 26 Dec 2024 14:25:41 +0800 Subject: [PATCH] update functions and logic of checking ACK --- protocol.c | 196 ++++++++++++++++++++++++++++++++++++----------------- protocol.h | 19 +++--- 2 files changed, 143 insertions(+), 72 deletions(-) diff --git a/protocol.c b/protocol.c index 2bc2a70..47edb1a 100644 --- a/protocol.c +++ b/protocol.c @@ -1,6 +1,21 @@ #include "protocol.h" #include +#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) { } /** - * 解��泵的告警信息 + * 解码泵的告警信息 * * @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; diff --git a/protocol.h b/protocol.h index ffdb0b9..e79049d 100644 --- a/protocol.h +++ b/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; -// 定义设备��认参数 +// 定义设备默认参数 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);