diff --git a/protocol.c b/protocol.c index 1b4e722..2d44d8d 100644 --- a/protocol.c +++ b/protocol.c @@ -87,7 +87,7 @@ void updateDeviceStatus(DeviceStatus_t status) { * 更新三通阀状态 * * @param index 阀门索引(1或2) - * @param angle 阀门角度值(120°或210°) + * @param angle 阀门角度值(0-360,绝对角度) */ void updateValveStatus(uint8_t index, ValveAngle_t angle) { if (index == 1) { @@ -322,7 +322,7 @@ uint8_t WritePump2Reg(uint8_t index, uint16_t reg, uint32_t value) { * @param acc 加速度值 * @return 0:成功 其他:失败 */ -static uint8_t WriteJogAcc(uint8_t index, uint16_t acc) { +static uint8_t SetPumpJogAcc(uint8_t index, uint16_t acc) { WritePump1Reg(index, RTU_PUMP_CMD_JA, acc); } @@ -333,7 +333,7 @@ static uint8_t WriteJogAcc(uint8_t index, uint16_t acc) { * @param dec 减速度值 * @return 0:成功 其他:失败 */ -static uint8_t WriteJogDec(uint8_t index, uint16_t dec) { +static uint8_t SetPumpJogDec(uint8_t index, uint16_t dec) { WritePump1Reg(index, RTU_PUMP_CMD_JL, dec); } @@ -344,14 +344,14 @@ static uint8_t WriteJogDec(uint8_t index, uint16_t dec) { * @param speed 速度值 * @return 0:成功 其他:失败 */ -static uint8_t WriteJogSpeed(uint8_t index, uint16_t speed) { +static uint8_t SetPumpJogSpeed(uint8_t index, uint16_t speed) { WritePump1Reg(index, RTU_PUMP_CMD_JS, speed); } // Jog=慢跑 // CJ=start jogging // 写入命令操作码寄存器(40125)数据0x0096(CJ),即执行启动Jog控制 -static uint8_t StartJogControl(uint8_t index) { +static uint8_t StartPumpJog(uint8_t index) { WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x0096); } @@ -359,7 +359,7 @@ static uint8_t StartJogControl(uint8_t index) { // 写入命令操作码寄存器(40125)数据0x00D8(SJ),即执行停止Jog控制 // CJ与SJ一一对应,单次SJ无法停止所有全部CJ // 直接停止泵需要使用SK命令 -static uint8_t StopJogControl(uint8_t index) { +static uint8_t StopPumpJog(uint8_t index) { WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x00D8); } @@ -383,7 +383,7 @@ static uint8_t StopJogControl(uint8_t index) { * @param acc 加速度值 * @return 0:成功 其他:失败 */ -static uint8_t WriteStepAcc(uint8_t index, uint16_t acc) { +static uint8_t SetPumpStepAcc(uint8_t index, uint16_t acc) { WritePump1Reg(index, RTU_PUMP_CMD_AC, acc); } /** @@ -393,7 +393,7 @@ static uint8_t WriteStepAcc(uint8_t index, uint16_t acc) { * @param dec 减速度值 * @return 0:成功 其他:失败 */ -static uint8_t WriteStepDec(uint8_t index, uint16_t dec) { +static uint8_t SetPumpStepDec(uint8_t index, uint16_t dec) { WritePump1Reg(index, RTU_PUMP_CMD_DE, dec); } /** @@ -404,7 +404,7 @@ static uint8_t WriteStepDec(uint8_t index, uint16_t dec) { * @param speed 速度百分比(0-100) * @return 0:成功 其他:失败 */ -static uint8_t WriteStepSpeed(uint8_t index, uint16_t speed) { +static uint8_t SetPumpStepSpeed(uint8_t index, uint16_t speed) { // 目标速度转换为实际速度 speed = (uint16_t)(speed * dp.pump[index].maxSpeed / 100); WritePump1Reg(index, RTU_PUMP_CMD_VE, speed); @@ -416,29 +416,29 @@ static uint8_t WriteStepSpeed(uint8_t index, uint16_t speed) { * @param target 目标位置值 * @return 0:成功 其他:失败 */ -static uint8_t WriteStepTarget(uint8_t index, uint32_t target) { +static uint8_t SetPumpStepTarget(uint8_t index, uint32_t target) { WritePump2Reg(index, RTU_PUMP_CMD_DI, target); } /** - * 执行相对位置控制 + * 执行相对位置控制,目前仅用到相对移动 * 写入FL(feed length)命令到操作码寄存器 * * @param index 泵索引 * @return 0:成功 其他:失败 */ -static uint8_t RelativePositionControl(uint8_t index) { +static uint8_t PumpRelativePositionControl(uint8_t index) { WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x0066); } /** - * 执行绝对位置控制 + * 执行绝对位置控制,目前无使用 * 写入FP(feed position)命令到操作码寄存器 * * @param index 泵索引 * @return 0:成功 其他:失败 */ -static uint8_t AbsolutePositionControl(uint8_t index) { +static uint8_t PumpAbsolutePositionControl(uint8_t index) { WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x0067); } @@ -459,7 +459,7 @@ static uint8_t StopPump(uint8_t index) { * @param index 泵索引 * @return 0:成功 其他:失败 */ -uint8_t ReadHWReg(uint8_t index) { +uint8_t ReadPumpHWReg(uint8_t index) { ReadPump1Reg(index, RTU_PUMP_CMD_HW); } /** @@ -469,7 +469,7 @@ uint8_t ReadHWReg(uint8_t index) { * @param br 波特率值 * @return 0:成功 其他:失败 */ -uint8_t WriteBRReg(uint8_t index, uint16_t br) { +uint8_t SetPumpBR(uint8_t index, uint16_t br) { WritePump1Reg(index, RTU_PUMP_CMD_BR, br); } /** @@ -479,7 +479,7 @@ uint8_t WriteBRReg(uint8_t index, uint16_t br) { * @param pr 协议类型值 * @return 0:成功 其他:失败 */ -uint8_t WritePRReg(uint8_t index, uint16_t pr) { +uint8_t SetPumpPR(uint8_t index, uint16_t pr) { WritePump1Reg(index, RTU_PUMP_CMD_PR, pr); } @@ -562,6 +562,45 @@ void DecodePumpStatusMsg(uint16_t reg4002) { return 0; } + + +/** + * 定时更新泵的状态 + * 读取泵的运行状态和告警信息 + */ +void UpdatePumpStatus() { + // 更新设备状态 + ReadPumpStatus(0); + ReadPumpStatus(1); + ReadPumpAlarm(0); + ReadPumpAlarm(1); +} + +/** + * 初始化泵参数 + * 设置最大速度、加速度和减速度 + * + * @return 0:成功 其他:失败 + */ +uint8_t InitPump(void) { + // 初始化泵 + log_e("InitPump"); + WriteJogAcc(dp.pump[0].id, dp.pump[0].maxAccel); + WriteJogDec(dp.pump[0].id, dp.pump[0].maxDecel); + WriteJogSpeed(dp.pump[0].id, dp.pump[0].maxSpeed); + WriteStepAcc(dp.pump[0].id, dp.pump[0].maxAccel); + WriteStepDec(dp.pump[0].id, dp.pump[0].maxDecel); + WriteStepSpeed(dp.pump[0].id, dp.pump[0].maxSpeed); + + WriteJogAcc(dp.pump[1].id, dp.pump[1].maxAccel); + WriteJogDec(dp.pump[1].id, dp.pump[1].maxDecel); + WriteJogSpeed(dp.pump[1].id, dp.pump[1].maxSpeed); + WriteStepAcc(dp.pump[1].id, dp.pump[1].maxAccel); + WriteStepDec(dp.pump[1].id, dp.pump[1].maxDecel); + WriteStepSpeed(dp.pump[1].id, dp.pump[1].maxSpeed); +} + + // valve /* # 轮廓位置模式,配置流程 @@ -807,15 +846,16 @@ uint8_t ValveRunInit(uint8_t index) { * 控制阀门运行到指定角度 * * @param index 阀门索引 - * @param angle 目标角度(0/120/210) + * @param angle 目标角度(0-360,绝对角度) * @return 0:成功 其他:失败 */ uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) { - // 限制角度为0,120,210 - if(angle != 0 && angle != 120 && angle != 210) { - printf("阀门角度设置错误\r\n"); + // 限制角度的逻辑不在这里,此处只执行控制逻辑 + if(angle > 360) { + log_e("阀门角度设置错误"); return 1; } + // 其它配置不变的情况下只需要写3个控制字 SetValvePPPos(index, (uint32_t)(angle*VALVE_PULSE_PER_ROUND/360)); // 电机以绝对位置,立即更新的方式运行 @@ -841,42 +881,6 @@ uint8_t InitValve(void) { SetValvePPDec(dp.valve[1].id, dp.valve[1].maxDecel); } -/** - * 定时更新泵的状态 - * 读取泵的运行状态和告警信息 - */ -void UpdatePumpStatus() { - // 更新设备状态 - ReadPumpStatus(0); - ReadPumpStatus(1); - ReadPumpAlarm(0); - ReadPumpAlarm(1); -} - -/** - * 初始化泵参数 - * 设置最大速度、加速度和减速度 - * - * @return 0:成功 其他:失败 - */ -uint8_t InitPump(void) { - // 初始化泵 - log_e("InitPump"); - WriteJogAcc(dp.pump[0].id, dp.pump[0].maxAccel); - WriteJogDec(dp.pump[0].id, dp.pump[0].maxDecel); - WriteJogSpeed(dp.pump[0].id, dp.pump[0].maxSpeed); - WriteStepAcc(dp.pump[0].id, dp.pump[0].maxAccel); - WriteStepDec(dp.pump[0].id, dp.pump[0].maxDecel); - WriteStepSpeed(dp.pump[0].id, dp.pump[0].maxSpeed); - - WriteJogAcc(dp.pump[1].id, dp.pump[1].maxAccel); - WriteJogDec(dp.pump[1].id, dp.pump[1].maxDecel); - WriteJogSpeed(dp.pump[1].id, dp.pump[1].maxSpeed); - WriteStepAcc(dp.pump[1].id, dp.pump[1].maxAccel); - WriteStepDec(dp.pump[1].id, dp.pump[1].maxDecel); - WriteStepSpeed(dp.pump[1].id, dp.pump[1].maxSpeed); -} - /** * 初始化设备状态 * 设置设备在线状态、阀门角度、泵运行状态等 @@ -916,7 +920,7 @@ static uint8_t HandleStatusQuery(void) { // 填充并返回数据 uint8_t txBuf[sizeof(DeviceStatus)] = {0}; memcpy(txBuf, &deviceStatus, sizeof(DeviceStatus)); - // sendMsgToHost(txBuf, sizeof(txBuf)); + sendMsgToHost(txBuf, sizeof(txBuf)); return 0; } @@ -936,7 +940,7 @@ static uint8_t HandleValveControl(uint8_t *Buff, uint8_t len) { return 0; } uint8_t index = Buff[0]; - uint8_t direction = Buff[1]; + uint8_t direction = Buff[1];//此状态位无效,目前三通阀有硬件限位,且指定角度必须为120或210 uint16_t angle = (Buff[2]<<8) | Buff[3]; if(angle > 360) { log_e("三通阀控制错误"); @@ -948,7 +952,7 @@ static uint8_t HandleValveControl(uint8_t *Buff, uint8_t len) { } // 具体实现 - + ValveRunToAngle(index,angle); // 更新三通阀状态 updateValveStatus(index, angle); @@ -991,7 +995,7 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) { log_e("泵速度设置错误"); return 0; } - WriteJogSpeed(index, speed); + SetPumpJogSpeed(index, speed); index = Buff[2]; speed = Buff[3]; @@ -999,7 +1003,7 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) { log_e("泵速度设置错误"); return 0; } - WriteJogSpeed(index, speed); + SetPumpJogSpeed(index, speed); return 1; } @@ -1019,7 +1023,7 @@ static uint8_t HandlePumpStepControl(uint8_t *Buff, uint8_t len) { } uint8_t index = Buff[0]; int32_t step = (Buff[1]<<24) | (Buff[2]<<16) | (Buff[3]<<8) | Buff[4]; - WriteStepTarget(index, step); + SetPumpStepTarget(index, step); return 0; } @@ -1045,8 +1049,8 @@ static uint8_t HandleSoftStop(uint8_t *rxBuf, uint16_t rxLen) { // 急停状态 StopPump(0); StopPump(1); - StopJogControl(0); - StopJogControl(1); + StopPumpJog(0); + StopPumpJog(1); updateEmergencyStop(ESTOP_PRESSED); } return 0; @@ -1120,22 +1124,22 @@ void packMsgToHost(uint16_t funcCode, uint8_t isOK) { CmdFrameError_t checkHostCmd(uint8_t *rxBuf, uint16_t rxLen) { // 检查命令是否正确 // FRAME_HEADER是按小端序存储的,而rxBuf是按大端序存的 - uint32_t header; - FillBigEndian32(&header, FRAME_HEADER); - uint32_t tail; - FillBigEndian32(&tail, FRAME_TAIL); + uint8_t header[sizeof(FRAME_HEADER)]; + FillBigEndian32(header, FRAME_HEADER); + uint8_t tail[sizeof(FRAME_TAIL)]; + FillBigEndian32(tail, FRAME_TAIL); - if(memcmp(rxBuf, &header, 4) != 0) + if(memcmp(rxBuf, header, sizeof(FRAME_HEADER)) != 0) { return CMD_FRAME_HEADER_ERROR; } - if (memcmp(rxBuf + rxLen - 4, &tail, 4) != 0) + if (memcmp(rxBuf + rxLen - sizeof(FRAME_TAIL), tail, sizeof(FRAME_TAIL)) != 0) { return CMD_FRAME_TAIL_ERROR; } - uint16_t crc = CalculateCRC16(rxBuf+4, rxLen - 8);// 计算crc,不包含帧头和帧尾 - if (memcmp(rxBuf + rxLen - 4, &crc, 2) != 0) + uint16_t crc = CalculateCRC16(rxBuf+sizeof(FRAME_HEADER), rxLen - sizeof(FRAME_HEADER)-sizeof(FRAME_TAIL))-2;// 计算crc,不包含帧头和帧尾和crc自身 + if ((rxBuf[rxLen-sizeof(FRAME_TAIL)-6]<<8) | rxBuf[rxLen-sizeof(FRAME_TAIL)-5] != crc) { return CMD_FRAME_CHECK_ERROR; }