149 lines
4.3 KiB
C
149 lines
4.3 KiB
C
/*
|
|
* @Date: 2025-06-26 14:49:09
|
|
* @Author: mypx
|
|
* @LastEditors: mypx mypx_coder@163.com
|
|
* @LastEditTime: 2025-08-29 09:08:23
|
|
* @FilePath: servo.c
|
|
* @Description:
|
|
* Copyright (c) 2025 by mypx, All Rights Reserved.
|
|
*/
|
|
#include "servo.h"
|
|
#include "nanomodbus.h"
|
|
#include "pm_board.h"
|
|
#include "rtdef.h"
|
|
#include "rtthread.h"
|
|
|
|
#if (ENABLE_SV630P == 1)
|
|
|
|
#define SERVO_RTU_ADDRESS 0x01
|
|
|
|
nmbs_t servo_nmbs;
|
|
sv630p_ops_t sv630p_ops;
|
|
sv630p_handle_t sv630p_handle;
|
|
|
|
#if (DEBUG_SERVO_ENABLE == 1)
|
|
#define DBG_SERVO(...) rt_kprintf("[sv630p] " __VA_ARGS__)
|
|
#else
|
|
#define DBG_SERVO(...)
|
|
#endif
|
|
|
|
int servo_read_holding_regs(sv630p_handle_t *dev, uint16_t address, uint16_t quantity, uint16_t *data)
|
|
{
|
|
nmbs_error ret = NMBS_ERROR_NONE;
|
|
|
|
ret = nmbs_read_holding_registers(dev->mb_handle, address, quantity, data);
|
|
if (ret != NMBS_ERROR_NONE)
|
|
{
|
|
DBG_SERVO("Read holding regs failed: address:0x%02X, quantity:%d, error:%d", address, quantity, ret);
|
|
return SV630P_MB_ERR;
|
|
}
|
|
DBG_SERVO("Read holding regs: address:0x%02X, quantity:%d success", address, quantity);
|
|
return SV630P_OK;
|
|
}
|
|
|
|
int servo_read_input_regs(sv630p_handle_t *dev, uint16_t address, uint16_t quantity, uint16_t *data)
|
|
{
|
|
nmbs_error ret = NMBS_ERROR_NONE;
|
|
|
|
ret = nmbs_read_input_registers(dev->mb_handle, address, quantity, data);
|
|
if (ret != NMBS_ERROR_NONE)
|
|
{
|
|
DBG_SERVO("Read input regs failed: address:0x%02X, quantity:%d, error:%d", address, quantity, ret);
|
|
return SV630P_MB_ERR;
|
|
}
|
|
DBG_SERVO("Read input regs: address:0x%02X, quantity:%d success", address, quantity);
|
|
return SV630P_OK;
|
|
}
|
|
|
|
int servo_write_single_reg(sv630p_handle_t *dev, uint16_t address, uint16_t value)
|
|
{
|
|
nmbs_error ret = NMBS_ERROR_NONE;
|
|
|
|
ret = nmbs_write_single_register(dev->mb_handle, address, value);
|
|
if (ret != NMBS_ERROR_NONE)
|
|
{
|
|
DBG_SERVO("Write single reg failed: address:0x%02X, value:%d, error:%d", address, value, ret);
|
|
return SV630P_MB_ERR;
|
|
}
|
|
DBG_SERVO("Write single reg: address:0x%02X, value:%d success", address, value);
|
|
return SV630P_OK;
|
|
}
|
|
|
|
int servo_write_multiple_regs(sv630p_handle_t *dev, uint16_t address, uint16_t quantity, uint16_t *data)
|
|
{
|
|
nmbs_error ret = NMBS_ERROR_NONE;
|
|
|
|
ret = nmbs_write_multiple_registers(dev->mb_handle, address, quantity, data);
|
|
if (ret != NMBS_ERROR_NONE)
|
|
{
|
|
DBG_SERVO("Write multiple regs failed: address:0x%02X, quantity:%d, error:%d", address, quantity, ret);
|
|
return SV630P_MB_ERR;
|
|
}
|
|
DBG_SERVO("Write multiple regs: address:0x%02X, quantity:%d success", address, quantity);
|
|
return SV630P_OK;
|
|
}
|
|
|
|
void servo_debug_printf(const char *format, ...)
|
|
{
|
|
rt_kprintf("[sv-debug] ");
|
|
rt_kprintf(format);
|
|
}
|
|
|
|
|
|
void servo_error_printf(const char *format, ...)
|
|
{
|
|
rt_kprintf("[sv-error] ");
|
|
rt_kprintf(format);
|
|
}
|
|
|
|
|
|
int servo_init(void)
|
|
{
|
|
int ret = 0;
|
|
nmbs_platform_conf stm32_conf;
|
|
// board layer init
|
|
pm_servo_init();
|
|
// nmbs layer init
|
|
nmbs_platform_conf_create(&stm32_conf);
|
|
stm32_conf.transport = NMBS_TRANSPORT_RTU;
|
|
stm32_conf.read = pm_servo_uart_read;
|
|
stm32_conf.write = pm_servo_uart_write;
|
|
|
|
nmbs_error status = nmbs_client_create(&servo_nmbs, &stm32_conf);
|
|
if (status != NMBS_ERROR_NONE)
|
|
{
|
|
return status;
|
|
}
|
|
nmbs_set_destination_rtu_address(&servo_nmbs, SERVO_RTU_ADDRESS);
|
|
nmbs_set_byte_timeout(&servo_nmbs, 1000);
|
|
nmbs_set_read_timeout(&servo_nmbs, 1000);
|
|
|
|
// sv630p driver init
|
|
sv630p_ops.read_holding_regs = servo_read_holding_regs;
|
|
sv630p_ops.read_input_regs = servo_read_input_regs;
|
|
sv630p_ops.write_single_reg = servo_write_single_reg;
|
|
sv630p_ops.write_multiple_regs = servo_write_multiple_regs;
|
|
|
|
|
|
sv630p_handle.trace.debug = servo_debug_printf; // set debug function
|
|
sv630p_handle.trace.error = servo_error_printf; // set error function
|
|
|
|
ret = sv630p_init(&sv630p_handle, &sv630p_ops, &servo_nmbs);
|
|
if (ret != 0)
|
|
{
|
|
DBG_SERVO("sv630p_init failed");
|
|
return -1;
|
|
}
|
|
sv630p_handle.init = true;
|
|
|
|
sv630p_speed_mode_stop(&sv630p_handle);
|
|
if (sv630p_speed_mode_init(&sv630p_handle) != 0)
|
|
{
|
|
DBG_SERVO("sv630p_speed_mode_init failed.");
|
|
return -1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
#endif // ENABLE_SV630P
|