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

update function

Browse Source
This commit is contained in:
murmur 2024-12-23 23:14:35 +08:00
parent 79f802e4b5
commit 614cb70134
2 changed files with 62 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
protocol.c
View File

@ -2,7 +2,7 @@
#include <string.h> #include <string.h>
DeviceStatus_t deviceStatus = { DeviceStatus_t deviceStatus = {
.deviceStatus = 1, .sensorStatus = 1,
.valves = {210, 120}, .valves = {210, 120},
.pumps = {0, 0, 50, 50}, .pumps = {0, 0, 50, 50},
.bubbleStatus = 0, .bubbleStatus = 0,
@ -10,8 +10,12 @@ DeviceStatus_t deviceStatus = {
.errorCode = 0, .errorCode = 0,
.initStatus = 1 .initStatus = 1
}; };
SystemStatus_t systemStatus = {
.speed = {0, 0},
.pos = {0, 0}
};
uint8_t isValveMovingBackToOrigin[2] = 0; uint8_t isValveMovingBackToOrigin[2] = {0,0};
DeviceParam_t dp = { DeviceParam_t dp = {
.pump = { .pump = {
@ -87,8 +91,8 @@ uint16_t CalculateCRC16(uint8_t *data, uint16_t length) {
* *
* @param status 设备状态值(在线/离线) * @param status 设备状态值(在线/离线)
*/ */
void updateDeviceStatus(DeviceStatus_t status) { void updateDeviceStatus(SensorStatus status) {
deviceStatus.deviceStatus = status; deviceStatus.sensorStatus = status;
} }
/** /**
@ -97,7 +101,7 @@ void updateDeviceStatus(DeviceStatus_t status) {
* @param index 阀门索引(1或2) * @param index 阀门索引(1或2)
* @param angle 阀门角度值(0-360，绝对角度) * @param angle 阀门角度值(0-360，绝对角度)
*/ */
void updateValveStatus(uint8_t index, ValveAngle_t angle) { void updateValveStatus(uint8_t index, ValveAngle angle) {
if (index == 1) { if (index == 1) {
deviceStatus.valves.angle1 = angle; deviceStatus.valves.angle1 = angle;
} else if (index == 2) { } else if (index == 2) {
@ -111,7 +115,7 @@ void updateValveStatus(uint8_t index, ValveAngle_t angle) {
* @param index 泵索引(1或2) * @param index 泵索引(1或2)
* @param status 泵状态值(停止/顺时针/逆时针) * @param status 泵状态值(停止/顺时针/逆时针)
*/ */
void updatePumpStatus(uint8_t index, PumpStatus_t status) { void updatePumpStatus(uint8_t index, PumpStatus status) {
if (index == 1) { if (index == 1) {
deviceStatus.pumps.status1 = status; deviceStatus.pumps.status1 = status;
} else if (index == 2) { } else if (index == 2) {
@ -138,7 +142,7 @@ void updatePumpSpeedStatus(uint8_t index, uint8_t speed) {
* *
* @param value 气泡传感器状态值(有/无气泡) * @param value 气泡传感器状态值(有/无气泡)
*/ */
void updateBubbleSensor(BubbleStatus_t value) { void updateBubbleSensor(BubbleStatus value) {
deviceStatus.bubbleStatus = value; deviceStatus.bubbleStatus = value;
} }
@ -312,7 +316,7 @@ uint32_t ReadPump2Reg(uint8_t id, uint16_t reg) {
/* /*
+----------+--------+------------+----------+------------+ +----------+--------+------------+----------+------------+
| 从机地址 | 功能码 | 寄存器地址 | 寄存器值 | CRC校验值 | | 从机地址 | 功能码 | 寄存器地址 | 寄存器<EFBFBD><EFBFBD><EFBFBD> | CRC校验值 |
+----------+--------+------------+----------+------------+ +----------+--------+------------+----------+------------+
| 1字节 | 1字节 | 2字节 | 2字节 | 2字节 | | 1字节 | 1字节 | 2字节 | 2字节 | 2字节 |
+----------+--------+------------+----------+------------+ +----------+--------+------------+----------+------------+
@ -555,7 +559,7 @@ uint16_t ReadPumpAlarm(uint8_t index) {
} }
/** /**
* 解码泵的告警信息 * 解<EFBFBD><EFBFBD>泵的告警信息
* *
* @param reg4001 告警寄存器值 * @param reg4001 告警寄存器值
*/ */
@ -690,7 +694,7 @@ uint8_t InitPump(void) {
2.4设置位移的减速度 (03FEh)(用户单位/s2)； 2.4设置位移的减速度 (03FEh)(用户单位/s2)；
3.写控制字使电机使能 3.写控制字使电机使能
(0380h)= 0x06→0x07→ 0x0F，电机使能： (0380h)= 0x06→0x07→ 0x0F，电机使能：
4.使电机运行 4.使电机<EFBFBD><EFBFBD>行
(0380h)= 0x2F→0x3F，电机运行 (0380h)= 0x2F→0x3F，电机运行
5.监控参数： 5.监控参数：
实际位置反馈：(03C8h) (用户单位) 实际位置反馈：(03C8h) (用户单位)
@ -711,10 +715,10 @@ uint8_t InitPump(void) {
*/ */
// 与pump通用 // 与pump通用
void (*writeValve1Reg)(uint8_t index, uint16_t reg, uint16_t value) = WritePump1Reg; uint8_t (*writeValve1Reg)(uint8_t index, uint16_t reg, uint16_t value) = WritePump1Reg;
void (*writeValve2Reg)(uint8_t index, uint16_t reg, uint32_t value) = WritePump2Reg; uint8_t (*writeValve2Reg)(uint8_t index, uint16_t reg, uint32_t value) = WritePump2Reg;
void (*readValve1Reg)(uint8_t index, uint16_t reg) = ReadPump1Reg; uint8_t (*readValve1Reg)(uint8_t index, uint16_t reg) = ReadPump1Reg;
void (*readValve2Reg)(uint8_t index, uint16_t reg) = ReadPump2Reg; uint8_t (*readValve2Reg)(uint8_t index, uint16_t reg) = ReadPump2Reg;
/** /**
* 读取阀门1个输入寄存器 * 读取阀门1个输入寄存器
@ -778,7 +782,7 @@ uint32_t ReadValve2InputReg(uint8_t id, uint16_t reg)
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t SetValveCOMMMode(uint8_t index, uint16_t mode) { static uint8_t SetValveCOMMMode(uint8_t index, uint16_t mode) {
return WriteValve1Reg(dp.valve[index].id, RTU_VALVE_CMD_CTL_MODE, mode); return writeValve1Reg(dp.valve[index].id, RTU_VALVE_CMD_CTL_MODE, mode);
} }
/** /**
@ -789,7 +793,7 @@ static uint8_t SetValveCOMMMode(uint8_t index, uint16_t mode) {
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
static uint8_t SetValveRunMode(uint8_t index, uint16_t mode) { static uint8_t SetValveRunMode(uint8_t index, uint16_t mode) {
return WriteValve1Reg(dp.valve[index].id, RTU_VALVE_CMD_RUN_MODE, mode); return writeValve1Reg(dp.valve[index].id, RTU_VALVE_CMD_RUN_MODE, mode);
} }
// PP=轮廓位置模式 // PP=轮廓位置模式
@ -931,9 +935,7 @@ static uint32_t ReadValvePos(uint8_t index) {
* @return 0:成功 1:运动中 2:失败 * @return 0:成功 1:运动中 2:失败
*/ */
static uint8_t valveCheckBTOResult(uint8_t index, uint32_t startTime) static uint8_t valveCheckBTOResult(uint8_t index, uint32_t startTime)
{ {
static uint32_t ts = HAL_GetTick();
if (HAL_GetTick() - startTime > 1000*20) { if (HAL_GetTick() - startTime > 1000*20) {
// 超时=失败 // 超时=失败
return 2; return 2;
@ -1057,13 +1059,9 @@ uint8_t ValveRunToAngle(uint8_t index, uint32_t angle) {
*/ */
uint8_t InitValve(void) { uint8_t InitValve(void) {
printf("InitValve\n"); printf("InitValve\n");
SetValvePPSpeed(0, dp.valve[0].maxSpeed);
SetValvePPAcc(0, dp.valve[0].maxAccel);
SetValvePPDec(0, dp.valve[0].maxDecel);
SetValvePPSpeed(1, dp.valve[1].maxSpeed); ValvePPInit(0);
SetValvePPAcc(1, dp.valve[1].maxAccel); ValvePPInit(1);
SetValvePPDec(1, dp.valve[1].maxDecel);
} }
/** /**
@ -1075,7 +1073,7 @@ void InitDeviceStatus() {
// 更新设备状态 // 更新设备状态
updateDeviceStatus(DEVICE_ONLINE); updateDeviceStatus(SENSOR_ONLINE);
updateValveStatus(1, 120); updateValveStatus(1, 120);
updateValveStatus(2, 210); updateValveStatus(2, 210);
updatePumpStatus(1, PUMP_CLOCKWISE); updatePumpStatus(1, PUMP_CLOCKWISE);
@ -1092,16 +1090,17 @@ void InitDeviceStatus() {
*/ */
void updateVPStatus(uint8_t index) void updateVPStatus(uint8_t index)
{ {
static uint32_t st[2] = {0, 0}; // Initialize with constants
// 检查是否回归成功
static uint32_t st[2] = {HAL_GetTick(),HAL_GetTick()};
if (isValveMovingBackToOrigin[index]) if (isValveMovingBackToOrigin[index])
{ {
uint8_t rst = valveCheckBTOResult(index,st[index]); if (st[index] == 0) { // First time initialization
st[index] = HAL_GetTick();
}
uint8_t rst = valveCheckBTOResult(index, st[index]);
switch (rst) switch (rst)
{ {
case 0: case 0:
log_i("BTO success!"); log_i("BTO success!delta=%d",HAL_GetTick()-st[index]);
updateInitStatus(INIT_SUCCESS); updateInitStatus(INIT_SUCCESS);
break; break;
case 1: case 1:
@ -1123,13 +1122,15 @@ void updateVPStatus(uint8_t index)
// 获取实时角度 // 获取实时角度
readPumpPos(index); readPumpPos(index);
uint32_t pos[index] = ReadValve2InputReg(index,RTU_VALVE_CMD_POS); uint32_t pos = ReadValve2InputReg(index,RTU_VALVE_CMD_POS);
log_d("pos[%d]=%d",index,pos[index]); log_d("pos[%d]=%d",index,pos);
systemStatus.pos[index] = pos;
// 获取运动状态 // 获取运动状态
uint32_t speed[index] = ReadValve2InputReg(index,RTU_VALVE_CMD_SPEED); uint32_t speed = ReadValve2InputReg(index,RTU_VALVE_CMD_SPEED);
log_d("speed[%d]=",index,speed[index]); log_d("speed[%d]=",index,speed);
systemStatus.speed[index] = speed;
} }
@ -1143,6 +1144,14 @@ void updateSystemStatus(void)
updateVPStatus(1); updateVPStatus(1);
} }
/**
* 初始化系统
*/
void initSystem(void)
{
systemStatus.ds = deviceStatus;
InitValve();
}
// 初始化处理 // 初始化处理
static uint8_t HandleInit(void) { static uint8_t HandleInit(void) {
// 实现初始化逻辑 // 实现初始化逻辑
@ -1205,7 +1214,7 @@ static uint8_t HandleValveControl(uint8_t *Buff, uint8_t len) {
/** /**
* 处理泵时长控制命令 * 处理泵时长控制命令
* *
* @param Buff 接收到的数据缓冲区 * @param Buff 接收<EFBFBD><EFBFBD>的数据缓冲区
* @param len 接收到的数据长度 * @param len 接收到的数据长度
* @return 0:成功 其他:失败 * @return 0:成功 其他:失败
*/ */
@ -1323,7 +1332,7 @@ void packMsgToHost(uint16_t funcCode, uint8_t isOK) {
FillBigEndian16(msgBuf+sizeof(FRAME_HEADER), funcCode); FillBigEndian16(msgBuf+sizeof(FRAME_HEADER), funcCode);
if(funcCode == HOST_CMD_STATUS_QUERY) { if(funcCode == HOST_CMD_STATUS_QUERY) {
dlen = sizeof(DeviceStatus); dlen = sizeof(DeviceStatus_t);
index = sizeof(FRAME_HEADER)+2; index = sizeof(FRAME_HEADER)+2;
msgBuf[index] = dlen; msgBuf[index] = dlen;
index += 1; index += 1;

30
protocol.h
View File

@ -334,20 +334,20 @@ typedef enum {
typedef enum { typedef enum {
SENSOR_OFFLINE = 0, SENSOR_OFFLINE = 0,
SENSOR_ONLINE = 1 SENSOR_ONLINE = 1
} SensorStatus_t; } SensorStatus;
// 2. 三通阀角度 // 2. 三通阀角度
typedef enum { typedef enum {
VALVE_ANGLE_120 = 120, VALVE_ANGLE_120 = 120,
VALVE_ANGLE_210 = 210 VALVE_ANGLE_210 = 210
} ValveAngle_t; } ValveAngle;
// 3. 泵状态 // 3. 泵状态
typedef enum { typedef enum {
PUMP_STOP = 0, PUMP_STOP = 0,
PUMP_CLOCKWISE = 1, PUMP_CLOCKWISE = 1,
PUMP_ANTICLOCKWISE = 2 PUMP_ANTICLOCKWISE = 2
} PumpStatus_t; } PumpStatus;
// 4. 泵速度范围 // 4. 泵速度范围
#define PUMP_SPEED_MIN 0 #define PUMP_SPEED_MIN 0
@ -357,7 +357,7 @@ typedef enum {
typedef enum { typedef enum {
BUBBLE_NONE = 0, BUBBLE_NONE = 0,
BUBBLE_DETECTED = 1 BUBBLE_DETECTED = 1
} BubbleStatus_t; } BubbleStatus;
// 6. 急停按键状态 // 6. 急停按键状态
typedef enum { typedef enum {
@ -388,9 +388,9 @@ typedef struct {
// 设备状态结构体，用于上报HOST // 设备状态结构体，用于上报HOST
typedef struct { typedef struct {
uint8_t deviceStatus; // 下挂设备状态 uint8_t sensorStatus; // 下挂设备状态
ValveStatus valves; // 两个三通阀状态 ValveStatus_t valves; // 两个三通阀状态
PumpStatus pumps; // 两个泵状态 PumpStatus_t pumps; // 两个泵状态
uint8_t bubbleStatus; // 气泡状态 uint8_t bubbleStatus; // 气泡状态
uint8_t stopStatus; // 急停状态 uint8_t stopStatus; // 急停状态
uint8_t errorCode; // 错误码 uint8_t errorCode; // 错误码
@ -401,11 +401,8 @@ typedef struct {
typedef struct typedef struct
{ {
DeviceStatus_t ds; DeviceStatus_t ds;
typedef struct uint32_t speed[2];//实时速度
{ uint32_t pos[2];//实时位置
uint32_t speed[2];//实时速度
uint32_t pos[2];//实时位置
};
} SystemStatus_t; } SystemStatus_t;
@ -426,8 +423,8 @@ typedef struct {
} DeviceParam_t; } DeviceParam_t;
extern DeviceParam dp; extern DeviceParam_t dp;
extern DeviceStatus deviceStatus; extern DeviceStatus_t deviceStatus;
// 函数声明 // 函数声明
CmdFrameError_t ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen); CmdFrameError_t ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen);
@ -439,4 +436,9 @@ void UpdatePumpStatus();
void ProcessMotorMsg(uint8_t *rxBuf, uint16_t rxLen); void ProcessMotorMsg(uint8_t *rxBuf, uint16_t rxLen);
void runPumpDemo(void); void runPumpDemo(void);
void runVavleDemo(void); void runVavleDemo(void);
void updateSystemStatus(void);
void initSystem(void);
#endif // PROTOCOL_H #endif // PROTOCOL_H