error
This commit is contained in:
CSSC-WORK\murmur
parent
33c8967a37
commit
7831469c66
150
protocol.c
150
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;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user