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

error

Browse Source
This commit is contained in:
CSSC-WORK\murmur 2024-12-13 17:08:28 +08:00
parent 33c8967a37
commit 7831469c66
1 changed files with 77 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

150
protocol.c
View File

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