hm/StepMotorCtrl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新：整机逻辑、ID指示器驱动、动态Modbus地址、指令文档

Browse Source
This commit is contained in:
hm-thinkbook14p
2026-05-11 22:00:49 +08:00
parent b49c9ab0c6
commit 20e3988eee
11 changed files with 614 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1
View File
@@ -26,6 +26,7 @@ include_directories(
${CMAKE_SOURCE_DIR}/bsp/device/eeprom
${CMAKE_SOURCE_DIR}/bsp/device/limit_switch
${CMAKE_SOURCE_DIR}/bsp/device/as5600
${CMAKE_SOURCE_DIR}/bsp/device/id
${CMAKE_SOURCE_DIR}/bsp/modbus
${CMAKE_SOURCE_DIR}/drv/system
${CMAKE_SOURCE_DIR}/drv/timer
README.md
+155
View File
@@ -0,0 +1,155 @@
# StepMotorCtrl - 步进电机控制系统
基于 **PY32MD530H28U7TR** 单片机的步进电机控制系统，适用于 claw machine（抓娃娃机）等场景，提供精准的电机位置控制、角度控制和标定功能。
## 硬件平台
| 项目 | 说明 |
|------|------|
| 主控芯片 | PY32MD530H28U7TR (Cortex-M0+) |
| 电机驱动 | TIM1 高分辨率 PWM，正弦/余弦微步进控制 |
| 角度传感器 | AS5600 (I2C1, 地址 0x36) |
| 存储器 | AT24C02 (I2C1, 地址 0x50) |
| 电流检测 | PGA + 内部 ADC |
| 通讯接口 | RS485 (USART1, Modbus RTU)、UART1 (调试)、LPUART1 (备份) |
| 到位开关 | DI_MIN (原点)、DI_MAX (最大值) |
| ID 指示器 | 4 位 GPIO (PB5~PB8)，支持 0~15 共 16 个地址 |
### 引脚分配
详见 [PinConfig.md](.trae/skills/create-project/references/PinConfig.md)
## 软件架构
```
StepMotorCtrl/
├── app/ # 应用层
│ ├── main.c # 主程序入口
│ ├── stepper/ # 步进电机控制
│ └── tasks/ # 任务模块（work_task, monitor_task）
├── bsp/ # 板级支持包
│ ├── device/ # 外设驱动
│ │ ├── as5600/ # 角度传感器
│ │ ├── eeprom/ # 存储器
│ │ ├── id/ # ID 指示器
│ │ ├── key/ # 按键
│ │ ├── led/ # LED
│ │ └── limit_switch/ # 到位开关
│ ├── modbus/ # Modbus RTU 从站
│ └── hardware_init.c/h # 硬件初始化
├── config/ # 配置文件
│ └── config.h # 引脚定义与参数配置
├── drv/ # 驱动层
│ ├── adc/ # ADC + PGA
│ ├── gpio/ # GPIO
│ ├── i2c/ # I2C
│ ├── log/ # 日志
│ ├── system/ # 系统时钟 + 数据总线
│ ├── timer/ # 定时器 PWM
│ └── uart/ # UART
└── libs/ # MCU 官方库
├── CMSIS/ # Cortex-M 内核头文件
└── device/ # PY32MD530 设备文件 + 链接脚本
```
## 构建
### 环境要求
- **编译器**: arm-none-eabi-gcc (含 newlib-nano)
- **构建工具**: CMake + MinGW Make (Windows) / make (Linux)
- **烧录工具**: 建议使用 PyOCD 或 J-Link
### 构建步骤
```bash
mkdir build && cd build
cmake .. -G "MinGW Makefiles"
mingw32-make -j4
```
生成文件：
- `StepMotorCtrl.elf` — ELF 可执行文件
- `StepMotorCtrl.hex` — HEX 烧录文件
- `StepMotorCtrl.bin` — BIN 烧录文件
## Modbus 通讯协议
| 参数 | 值 |
|------|-----|
| 物理层 | RS485 半双工 |
| 波特率 | 115200, 8N1 |
| 协议 | Modbus RTU |
| 主站地址 | 0xFE |
| 从站地址 | 0x40 + ID (ID 由 PB5~PB8 引脚读取) |
### 主要寄存器
| 地址 | 名称 | 说明 |
|------|------|------|
| 0x0001 | MOTOR_POSITION | 当前位置 (步数) |
| 0x0002 | MOTOR_TARGET | 目标位置 (步数) |
| 0x0005 | AS5600_ANGLE | 角度值 (0.1°, 0~3599) |
| 0x000F | DEVICE_ID | 设备 ID (0~15) |
| 0x0042 | CMD | 指令寄存器 |
| 0x0043 | STATUS | 整机状态 |
| 0x0044 | ANGLE_TARGET | 角度目标 (0.1°, 0~3600) |
### 指令码
| 码值 | 指令 | 说明 |
|------|------|------|
| 0x01 | GO_MAX | 运行到最大值 |
| 0x02 | GO_MIN | 运行到最小值 (原点) |
| 0x03 | STOP | 停止运行 |
| 0x04 | GO_POSITION | 运行到指定步数 |
| 0x05 | GO_ANGLE | 运行到指定角度 |
| 0x06 | CALIBRATE | 开始标定 |
| 0x07 | CALIB_END | 结束标定 |
### 通讯示例
```hex
# 读当前位置
请求: FE 03 00 01 00 01 CRC_LO CRC_HI
响应: 40+ID 03 02 POS_HI POS_LO CRC_LO CRC_HI
# 运行到最大值
请求: FE 06 00 42 00 01 CRC_LO CRC_HI
响应: 40+ID 06 00 42 00 01 CRC_LO CRC_HI
# 运行到指定角度 (45.0°)
请求: FE 06 00 44 01 C2 CRC_LO CRC_HI <- ANGLE_TARGET = 450
请求: FE 06 00 42 00 05 CRC_LO CRC_HI <- CMD = GO_ANGLE
```
详见 [command.md](.trae/skills/create-project/references/command.md)
## 功能说明
### 步进电机控制
- S 型加减速算法，支持加速、匀速、减速三个阶段
- 正弦/余弦微步进驱动，每电周期 1024 个微步
- 支持绝对位置移动和相对位置移动
- 到位自动停止，支持紧急停止
### 标定功能
- 自动标定：运行到 DI_MIN 和 DI_MAX 开关位置，记录步数
- 无 DI_MAX 时：根据原点开关和标定步数计算导轨长度
### 整机逻辑
- 上电初始化后进入待机模式，等待 Modbus 指令
- 支持指令队列和状态上报
- LED 指示灯反馈：待机 (500ms 闪烁)、运行 (100ms 闪烁)、标定 (80ms 闪烁)
## 功能文档
| 文档 | 说明 |
|------|------|
| [function.md](.trae/skills/create-project/references/function.md) | 功能规格与运行逻辑 |
| [command.md](.trae/skills/create-project/references/command.md) | Modbus 指令集与通讯协议 |
| [PinConfig.md](.trae/skills/create-project/references/PinConfig.md) | 引脚配置表 |
## License
Proprietary
app/main.c
+12 -1
View File
@@ -20,6 +20,7 @@
#endif
#include "as5600_driver.h"
#include "modbus_slave.h"
#include "id_driver.h"
#include "stepper_control.h"
#include "tasks/work_task.h"
#include "tasks/monitor_task.h"
@@ -188,7 +189,17 @@ int main(void)
as5600_init();
modbus_slave_init(MODBUS_SLAVE_ADDR);
id_driver_init();
{
sys_data_bus_value_t id_val;
id_val.u = id_get_value();
sys_data_bus_write(SYS_DATA_BUS_DEVICE_ID, id_val);
id_val.u = id_get_slave_address();
sys_data_bus_write(SYS_DATA_BUS_MODBUS_SLAVE_ADDR, id_val);
}
modbus_slave_init(id_get_slave_address());
stepper_init();
app/stepper/stepper_control.c
+42
View File
@@ -560,3 +560,45 @@ stepper_state_t stepper_get_state(void)
{
return state;
}
ret_code_t stepper_move_to_min(void)
{
return stepper_move_to(origin_steps);
}
ret_code_t stepper_move_to_max(void)
{
return stepper_move_to(max_steps);
}
ret_code_t stepper_move_to_angle(float angle_deg)
{
int32_t steps;
if (angle_deg < 0.0f) angle_deg = 0.0f;
if (angle_deg > 360.0f) angle_deg = 360.0f;
steps = (int32_t)((calibrated_steps * angle_deg) / 360.0f);
return stepper_move_to(origin_steps + steps);
}
uint8_t stepper_is_running(void)
{
return running;
}
int32_t stepper_get_max_steps(void)
{
return max_steps;
}
int32_t stepper_get_origin_steps(void)
{
return origin_steps;
}
uint8_t stepper_is_calibrated(void)
{
return calibrated;
}
app/stepper/stepper_control.h
+23
View File
@@ -13,6 +13,22 @@
#define STEPPER_PWM_CENTER 1800
#define STEPPER_HOLD_CURRENT 50
#define STEPPER_CMD_NONE 0x0000
#define STEPPER_CMD_GO_MAX 0x0001
#define STEPPER_CMD_GO_MIN 0x0002
#define STEPPER_CMD_STOP 0x0003
#define STEPPER_CMD_GO_POSITION 0x0004
#define STEPPER_CMD_GO_ANGLE 0x0005
#define STEPPER_CMD_CALIBRATE 0x0006
#define STEPPER_CMD_CALIB_END 0x0007
#define STEPPER_CMD_READ_STATUS 0x0008
#define STEPPER_STATUS_IDLE 0x0000
#define STEPPER_STATUS_RUNNING 0x0001
#define STEPPER_STATUS_CALIB 0x0002
#define STEPPER_STATUS_STOPPED 0x0003
#define STEPPER_STATUS_ERROR 0x0004
typedef enum {
STEPPER_STATE_IDLE = 0,
STEPPER_STATE_ACCEL,
@@ -31,13 +47,20 @@ ret_code_t stepper_init(void);
ret_code_t stepper_deinit(void);
ret_code_t stepper_move_to(int32_t target);
ret_code_t stepper_move_rel(int32_t steps);
ret_code_t stepper_move_to_min(void);
ret_code_t stepper_move_to_max(void);
ret_code_t stepper_move_to_angle(float angle_deg);
ret_code_t stepper_stop(void);
ret_code_t stepper_emergency_stop(void);
ret_code_t stepper_calibrate(void);
uint8_t stepper_is_running(void);
void stepper_run(void);
int32_t stepper_get_position(void);
int32_t stepper_get_target(void);
int32_t stepper_get_max_steps(void);
int32_t stepper_get_origin_steps(void);
uint8_t stepper_is_calibrated(void);
stepper_state_t stepper_get_state(void);
#endif
app/tasks/work_task.c
+201 -1
View File
@@ -1,23 +1,223 @@
#include "work_task.h"
#include "system_driver.h"
#include "sys_data_bus.h"
#include "stepper_control.h"
#include "limit_switch.h"
#include "led_driver.h"
#include "config.h"
#define DI_MIN LIMIT_SW_MIN
#define DI_MAX LIMIT_SW_MAX
#define WORK_TASK_LED_IDLE_MS 500
#define WORK_TASK_LED_RUN_MS 100
#define WORK_TASK_LED_CALIB_MS 80
#define WORK_TASK_LED_STOPPED_MS 0
typedef enum {
MACHINE_STANDBY = 0,
MACHINE_MOVING,
MACHINE_CALIBRATING,
MACHINE_STOPPED,
} machine_mode_t;
static uint32_t last_run_tick;
static uint32_t run_count;
static machine_mode_t machine_mode;
static uint16_t last_cmd;
static uint32_t led_toggle_tick;
static uint8_t led_state_on;
static void work_update_data_bus(void)
{
sys_data_bus_value_t val;
val.i = stepper_get_position();
sys_data_bus_write(SYS_DATA_BUS_MOTOR_POSITION, val);
val.i = stepper_get_target();
sys_data_bus_write(SYS_DATA_BUS_MOTOR_TARGET, val);
val.u = (uint32_t)stepper_get_state();
sys_data_bus_write(SYS_DATA_BUS_MOTOR_STATE, val);
val.u = limit_switch_get_state(DI_MIN) == LIMIT_SW_STATE_TRIGGERED ? 1 : 0;
sys_data_bus_write(SYS_DATA_BUS_DI_MIN_STATE, val);
val.u = limit_switch_get_state(DI_MAX) == LIMIT_SW_STATE_TRIGGERED ? 1 : 0;
sys_data_bus_write(SYS_DATA_BUS_DI_MAX_STATE, val);
}
static void work_update_status(void)
{
sys_data_bus_value_t val;
switch (machine_mode) {
case MACHINE_STANDBY:
val.u = STEPPER_STATUS_IDLE;
break;
case MACHINE_MOVING:
val.u = STEPPER_STATUS_RUNNING;
break;
case MACHINE_CALIBRATING:
val.u = STEPPER_STATUS_CALIB;
break;
case MACHINE_STOPPED:
val.u = STEPPER_STATUS_STOPPED;
break;
default:
val.u = STEPPER_STATUS_IDLE;
break;
}
sys_data_bus_write(SYS_DATA_BUS_MOTOR_STATUS, val);
}
static uint16_t work_read_command(void)
{
sys_data_bus_value_t val;
val = sys_data_bus_read(SYS_DATA_BUS_MOTOR_CMD);
return (uint16_t)(val.u & 0xFFFF);
}
static void work_clear_command(void)
{
sys_data_bus_value_t val;
val.u = STEPPER_CMD_NONE;
sys_data_bus_write(SYS_DATA_BUS_MOTOR_CMD, val);
}
static void work_dispatch_command(uint16_t cmd)
{
switch (cmd) {
case STEPPER_CMD_GO_MAX:
machine_mode = MACHINE_MOVING;
stepper_move_to_max();
break;
case STEPPER_CMD_GO_MIN:
machine_mode = MACHINE_MOVING;
stepper_move_to_min();
break;
case STEPPER_CMD_STOP:
machine_mode = MACHINE_STOPPED;
stepper_stop();
break;
case STEPPER_CMD_GO_POSITION:
{
sys_data_bus_value_t target;
target = sys_data_bus_read(SYS_DATA_BUS_MOTOR_TARGET);
machine_mode = MACHINE_MOVING;
stepper_move_to(target.i);
break;
}
case STEPPER_CMD_GO_ANGLE:
{
sys_data_bus_value_t angle_reg;
angle_reg = sys_data_bus_read(SYS_DATA_BUS_ANGLE_TARGET);
machine_mode = MACHINE_MOVING;
stepper_move_to_angle((float)angle_reg.u * 0.1f);
break;
}
case STEPPER_CMD_CALIBRATE:
machine_mode = MACHINE_CALIBRATING;
stepper_calibrate();
break;
case STEPPER_CMD_CALIB_END:
if (machine_mode == MACHINE_CALIBRATING) {
stepper_stop();
}
machine_mode = MACHINE_STOPPED;
break;
case STEPPER_CMD_READ_STATUS:
break;
default:
break;
}
}
static void work_led_update(void)
{
uint32_t now;
uint32_t interval;
now = system_get_tick();
switch (machine_mode) {
case MACHINE_STANDBY:
interval = WORK_TASK_LED_IDLE_MS;
break;
case MACHINE_MOVING:
interval = WORK_TASK_LED_RUN_MS;
break;
case MACHINE_CALIBRATING:
interval = WORK_TASK_LED_CALIB_MS;
break;
case MACHINE_STOPPED:
led_set(LED_ON);
return;
default:
interval = WORK_TASK_LED_IDLE_MS;
break;
}
if ((now - led_toggle_tick) >= interval) {
led_toggle_tick = now;
led_state_on = !led_state_on;
led_set(led_state_on ? LED_ON : LED_OFF);
}
}
void work_task_init(void)
{
last_run_tick = 0;
run_count = 0;
machine_mode = MACHINE_STANDBY;
last_cmd = STEPPER_CMD_NONE;
led_toggle_tick = 0;
led_state_on = 0;
led_init();
}
void work_task_run(void)
{
uint16_t cmd;
last_run_tick = system_get_tick();
run_count++;
led_blink(500);
work_update_data_bus();
work_update_status();
work_led_update();
cmd = work_read_command();
if (cmd != STEPPER_CMD_NONE && cmd != last_cmd) {
last_cmd = cmd;
work_clear_command();
work_dispatch_command(cmd);
}
if (machine_mode == MACHINE_MOVING && !stepper_is_running()) {
machine_mode = MACHINE_STANDBY;
}
if (machine_mode == MACHINE_CALIBRATING && !stepper_is_running()) {
machine_mode = MACHINE_STANDBY;
}
}
uint32_t work_task_get_last_run_tick(void)
bsp/device/id/id_driver.c
+34
View File
@@ -0,0 +1,34 @@
#include "id_driver.h"
#include "gpio_driver.h"
#include "config.h"
static uint8_t current_id;
static uint8_t id_read_pins(void)
{
uint8_t val = 0;
if (gpio_get_level(ID_PORT, ID_PIN_1)) val |= 0x01;
if (gpio_get_level(ID_PORT, ID_PIN_2)) val |= 0x02;
if (gpio_get_level(ID_PORT, ID_PIN_3)) val |= 0x04;
if (gpio_get_level(ID_PORT, ID_PIN_4)) val |= 0x08;
return val;
}
ret_code_t id_driver_init(void)
{
current_id = id_read_pins();
return RET_OK;
}
uint8_t id_get_value(void)
{
return current_id;
}
uint8_t id_get_slave_address(void)
{
return ID_BASE_ADDR + current_id;
}
bsp/device/id/id_driver.h
+14
View File
@@ -0,0 +1,14 @@
#ifndef ID_DRIVER_H__
#define ID_DRIVER_H__
#include "common_types.h"
#include <stdint.h>
#define ID_PIN_COUNT 4
#define ID_BASE_ADDR 0x40
ret_code_t id_driver_init(void);
uint8_t id_get_value(void);
uint8_t id_get_slave_address(void);
#endif
bsp/modbus/modbus_slave.c
+127 -61
View File
@@ -5,40 +5,48 @@
#include "config.h"
#include <string.h>
#define MODBUS_RX_TIMEOUT_MS 5
#define MODBUS_TX_GUARD_US 2
#define MODBUS_RX_TIMEOUT_MS 5
#define MODBUS_TX_GUARD_US 2
#define MODBUS_EXCEPTION_ILLEGAL_FUNCTION 0x01
#define MODBUS_EXCEPTION_ILLEGAL_DATA_ADDR 0x02
#define MODBUS_EXCEPTION_ILLEGAL_DATA_VAL 0x03
#define MODBUS_EXCEPTION_ILLEGAL_FUNCTION 0x01
#define MODBUS_EXCEPTION_ILLEGAL_DATA_ADDR 0x02
#define MODBUS_EXCEPTION_ILLEGAL_DATA_VAL 0x03
#define MODBUS_ADDR_BROADCAST 0x00
#define MODBUS_ADDR_BROADCAST 0x00
#define REG_BUS_DATA_START 0x0000
#define REG_BUS_DATA_END 0x001F
#define REG_BUS_DATA_HIGH_START 0x0020
#define REG_BUS_DATA_HIGH_END 0x003F
#define REG_FLAG_LOW 0x0040
#define REG_FLAG_HIGH 0x0041
#define REG_BUS_DATA_START 0x0000
#define REG_BUS_DATA_END 0x001F
#define REG_BUS_DATA_HIGH_START 0x0020
#define REG_BUS_DATA_HIGH_END 0x003F
#define REG_FLAG_LOW 0x0040
#define REG_FLAG_HIGH 0x0041
#define REG_CMD 0x0042
#define REG_STATUS 0x0043
#define REG_ANGLE_TARGET 0x0044
static uint8_t slave_address;
static uint8_t slave_address;
static modbus_state_t state;
static uint8_t rx_buf[MODBUS_BUF_SIZE];
static uint16_t rx_len;
static uint32_t rx_tick_start;
static uint8_t initialized;
static uint8_t rx_buf[MODBUS_BUF_SIZE];
static uint16_t rx_len;
static uint32_t rx_tick_start;
static uint8_t initialized;
static uint16_t modbus_crc16_calc(const uint8_t *data, uint16_t len)
{
uint16_t crc = 0xFFFF;
uint16_t i, j;
for (i = 0; i < len; i++) {
for (i = 0; i < len; i++)
{
crc ^= data[i];
for (j = 0; j < 8; j++) {
if (crc & 0x0001) {
for (j = 0; j < 8; j++)
{
if (crc & 0x0001)
{
crc = (crc >> 1) ^ 0xA001;
} else {
}
else
{
crc = crc >> 1;
}
}
@@ -81,36 +89,61 @@ static void modbus_handle_read_holding(uint16_t reg_addr, uint16_t reg_count)
sys_data_bus_value_t bus_val;
uint32_t flags;
if (reg_count < 1 || reg_count > 125) {
if (reg_count < 1 || reg_count > 125)
{
modbus_send_exception(MODBUS_FUNC_READ_HOLDING, MODBUS_EXCEPTION_ILLEGAL_DATA_VAL);
return;
}
if ((reg_addr >= REG_BUS_DATA_START && (reg_addr + reg_count - 1) <= REG_BUS_DATA_END) ||
(reg_addr >= REG_BUS_DATA_HIGH_START && (reg_addr + reg_count - 1) <= REG_BUS_DATA_HIGH_END) ||
(reg_addr >= REG_FLAG_LOW && (reg_addr + reg_count - 1) <= REG_FLAG_HIGH)) {
(reg_addr >= REG_FLAG_LOW && (reg_addr + reg_count - 1) <= REG_ANGLE_TARGET))
{
resp[0] = slave_address;
resp[1] = MODBUS_FUNC_READ_HOLDING;
resp[2] = (uint8_t)(reg_count * 2);
for (i = 0; i < reg_count; i++) {
for (i = 0; i < reg_count; i++)
{
uint16_t addr = reg_addr + i;
uint16_t reg_val = 0;
if (addr <= REG_BUS_DATA_END) {
if (addr <= REG_BUS_DATA_END)
{
bus_val = sys_data_bus_read((sys_data_bus_id_t)addr);
reg_val = (uint16_t)(bus_val.u & 0xFFFF);
} else if (addr >= REG_BUS_DATA_HIGH_START && addr <= REG_BUS_DATA_HIGH_END) {
}
else if (addr >= REG_BUS_DATA_HIGH_START && addr <= REG_BUS_DATA_HIGH_END)
{
bus_val = sys_data_bus_read((sys_data_bus_id_t)(addr - REG_BUS_DATA_HIGH_START));
reg_val = (uint16_t)((bus_val.u >> 16) & 0xFFFF);
} else if (addr == REG_FLAG_LOW) {
}
else if (addr == REG_FLAG_LOW)
{
flags = 0;
reg_val = (uint16_t)(flags & 0xFFFF);
} else if (addr == REG_FLAG_HIGH) {
}
else if (addr == REG_FLAG_HIGH)
{
flags = 0;
reg_val = (uint16_t)((flags >> 16) & 0xFFFF);
}
else if (addr == REG_CMD)
{
bus_val = sys_data_bus_read(SYS_DATA_BUS_MOTOR_CMD);
reg_val = (uint16_t)(bus_val.u & 0xFFFF);
}
else if (addr == REG_STATUS)
{
bus_val = sys_data_bus_read(SYS_DATA_BUS_MOTOR_STATUS);
reg_val = (uint16_t)(bus_val.u & 0xFFFF);
}
else if (addr == REG_ANGLE_TARGET)
{
bus_val = sys_data_bus_read(SYS_DATA_BUS_ANGLE_TARGET);
reg_val = (uint16_t)(bus_val.u & 0xFFFF);
}
resp[3 + i * 2] = (uint8_t)((reg_val >> 8) & 0xFF);
resp[4 + i * 2] = (uint8_t)(reg_val & 0xFF);
@@ -122,7 +155,9 @@ static void modbus_handle_read_holding(uint16_t reg_addr, uint16_t reg_count)
resp[resp_len + 1] = (uint8_t)((crc >> 8) & 0xFF);
modbus_send_response(resp, resp_len + 2);
} else {
}
else
{
modbus_send_exception(MODBUS_FUNC_READ_HOLDING, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDR);
}
}
@@ -132,14 +167,17 @@ static void modbus_handle_write_single_reg(uint16_t reg_addr, uint16_t reg_value
uint8_t resp[8];
uint16_t crc;
if (reg_addr <= REG_BUS_DATA_END) {
if (reg_addr <= REG_BUS_DATA_END)
{
sys_data_bus_value_t val;
sys_data_bus_value_t current;
current = sys_data_bus_read((sys_data_bus_id_t)reg_addr);
val.u = (current.u & 0xFFFF0000) | reg_value;
sys_data_bus_write((sys_data_bus_id_t)reg_addr, val);
} else if (reg_addr >= REG_BUS_DATA_HIGH_START && reg_addr <= REG_BUS_DATA_HIGH_END) {
}
else if (reg_addr >= REG_BUS_DATA_HIGH_START && reg_addr <= REG_BUS_DATA_HIGH_END)
{
sys_data_bus_value_t val;
sys_data_bus_value_t current;
uint8_t idx = (uint8_t)(reg_addr - REG_BUS_DATA_HIGH_START);
@@ -147,7 +185,21 @@ static void modbus_handle_write_single_reg(uint16_t reg_addr, uint16_t reg_value
current = sys_data_bus_read((sys_data_bus_id_t)idx);
val.u = (current.u & 0x0000FFFF) | ((uint32_t)reg_value << 16);
sys_data_bus_write((sys_data_bus_id_t)idx, val);
} else {
}
else if (reg_addr == REG_CMD)
{
sys_data_bus_value_t val;
val.u = reg_value;
sys_data_bus_write(SYS_DATA_BUS_MOTOR_CMD, val);
}
else if (reg_addr == REG_ANGLE_TARGET)
{
sys_data_bus_value_t val;
val.u = reg_value;
sys_data_bus_write(SYS_DATA_BUS_ANGLE_TARGET, val);
}
else
{
modbus_send_exception(MODBUS_FUNC_WRITE_SINGLE_REG, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDR);
return;
}
@@ -170,12 +222,14 @@ static void modbus_process_frame(void)
{
uint16_t crc_recv, crc_calc;
if (rx_len < 4) {
if (rx_len < 4)
{
state = MODBUS_IDLE;
return;
}
if (rx_buf[0] != slave_address && rx_buf[0] != MODBUS_ADDR_BROADCAST) {
if (rx_buf[0] != slave_address && rx_buf[0] != MODBUS_ADDR_BROADCAST)
{
state = MODBUS_IDLE;
return;
}
@@ -183,7 +237,8 @@ static void modbus_process_frame(void)
crc_recv = (uint16_t)rx_buf[rx_len - 1] << 8 | rx_buf[rx_len - 2];
crc_calc = modbus_crc16_calc(rx_buf, rx_len - 2);
if (crc_recv != crc_calc) {
if (crc_recv != crc_calc)
{
state = MODBUS_IDLE;
return;
}
@@ -193,28 +248,31 @@ static void modbus_process_frame(void)
uint16_t reg_addr;
uint16_t reg_count;
switch (func) {
case MODBUS_FUNC_READ_HOLDING:
if (rx_len < 8) {
break;
}
reg_addr = (uint16_t)rx_buf[2] << 8 | rx_buf[3];
reg_count = (uint16_t)rx_buf[4] << 8 | rx_buf[5];
modbus_handle_read_holding(reg_addr, reg_count);
switch (func)
{
case MODBUS_FUNC_READ_HOLDING:
if (rx_len < 8)
{
break;
}
reg_addr = (uint16_t)rx_buf[2] << 8 | rx_buf[3];
reg_count = (uint16_t)rx_buf[4] << 8 | rx_buf[5];
modbus_handle_read_holding(reg_addr, reg_count);
break;
case MODBUS_FUNC_WRITE_SINGLE_REG:
if (rx_len < 8) {
break;
}
reg_addr = (uint16_t)rx_buf[2] << 8 | rx_buf[3];
reg_count = (uint16_t)rx_buf[4] << 8 | rx_buf[5];
modbus_handle_write_single_reg(reg_addr, reg_count);
case MODBUS_FUNC_WRITE_SINGLE_REG:
if (rx_len < 8)
{
break;
}
reg_addr = (uint16_t)rx_buf[2] << 8 | rx_buf[3];
reg_count = (uint16_t)rx_buf[4] << 8 | rx_buf[5];
modbus_handle_write_single_reg(reg_addr, reg_count);
break;
default:
modbus_send_exception(func, MODBUS_EXCEPTION_ILLEGAL_FUNCTION);
break;
default:
modbus_send_exception(func, MODBUS_EXCEPTION_ILLEGAL_FUNCTION);
break;
}
}
@@ -245,20 +303,25 @@ void modbus_slave_poll(void)
uint8_t byte;
uint32_t now;
if (!initialized) {
if (!initialized)
{
return;
}
available = uart_available(UART_ID_USART1);
if (available > 0) {
if (state == MODBUS_IDLE) {
if (available > 0)
{
if (state == MODBUS_IDLE)
{
state = MODBUS_RX_IN_PROGRESS;
rx_len = 0;
}
while (available > 0 && rx_len < MODBUS_BUF_SIZE) {
if (uart_receive(UART_ID_USART1, &byte, 1, 0) == RET_OK) {
while (available > 0 && rx_len < MODBUS_BUF_SIZE)
{
if (uart_receive(UART_ID_USART1, &byte, 1, 0) == RET_OK)
{
rx_buf[rx_len++] = byte;
}
available--;
@@ -267,14 +330,17 @@ void modbus_slave_poll(void)
rx_tick_start = system_get_tick();
}
if (state == MODBUS_RX_IN_PROGRESS && rx_len > 0) {
if (state == MODBUS_RX_IN_PROGRESS && rx_len > 0)
{
now = system_get_tick();
if ((now - rx_tick_start) >= MODBUS_RX_TIMEOUT_MS) {
if ((now - rx_tick_start) >= MODBUS_RX_TIMEOUT_MS)
{
state = MODBUS_RX_COMPLETE;
}
}
if (state == MODBUS_RX_COMPLETE) {
if (state == MODBUS_RX_COMPLETE)
{
modbus_process_frame();
}
}
bsp/modbus/modbus_slave.h
+1 -1
View File
@@ -5,7 +5,7 @@
#include <stdint.h>
#define MODBUS_SLAVE_ADDR 0x01
#define MODBUS_MASTER_ADDR 0xF0
#define MODBUS_MASTER_ADDR 0xFE
#define MODBUS_BUF_SIZE 256
#define MODBUS_FUNC_READ_COILS 0x01
drv/system/sys_data_bus.h
+4
View File
@@ -22,6 +22,10 @@ typedef enum {
SYS_DATA_BUS_IS_CALIBRATED,
SYS_DATA_BUS_IS_DI_MAX_INSTALLED,
SYS_DATA_BUS_MODBUS_SLAVE_ADDR,
SYS_DATA_BUS_DEVICE_ID,
SYS_DATA_BUS_MOTOR_CMD,
SYS_DATA_BUS_MOTOR_STATUS,
SYS_DATA_BUS_ANGLE_TARGET,
} sys_data_bus_id_t;
typedef union {