83 lines
2.0 KiB
C
83 lines
2.0 KiB
C
/*
|
|
* @Author: mypx
|
|
* @Date: 2025-10-15 16:40:13
|
|
* @LastEditTime: 2025-10-15 16:47:38
|
|
* @LastEditors: mypx mypx_coder@163.com
|
|
* @Description:
|
|
*/
|
|
#include "bsp_motor.h"
|
|
#include "et_log.h"
|
|
#include "rtthread.h"
|
|
#include "usart.h"
|
|
|
|
#if (DEBUG_MB_ENABLE == 1)
|
|
#define DBG_MB(...) ET_DBG("[MB] " __VA_ARGS__)
|
|
#else
|
|
#define DBG_MB(...)
|
|
#endif
|
|
|
|
struct rt_messagequeue servo_mq;
|
|
static char mq_pool[SERVO_RX_QUEUE_SIZE];
|
|
|
|
|
|
int bsp_servo_init(void)
|
|
{
|
|
MX_USART3_UART_Init();
|
|
|
|
rt_err_t result = rt_mq_init(&servo_mq, "servo_mq", mq_pool, 1, SERVO_RX_QUEUE_SIZE, RT_IPC_FLAG_FIFO);
|
|
if (result != RT_EOK)
|
|
{
|
|
ET_ERR("servo mq init failed\n");
|
|
return -1;
|
|
}
|
|
|
|
__HAL_UART_ENABLE_IT(&SERVO_COM, UART_IT_RXNE);
|
|
return 0;
|
|
}
|
|
|
|
int32_t bsp_servo_uart_read(uint8_t *buf, uint16_t count, int32_t byte_timeout_ms, void *arg)
|
|
{
|
|
UNUSED(arg);
|
|
uint16_t bytes_read = 0;
|
|
uint8_t byte;
|
|
rt_tick_t timeout_ticks = rt_tick_from_millisecond(byte_timeout_ms);
|
|
DBG_MB("start read:count:%d, timeout:%d\n", count, byte_timeout_ms);
|
|
|
|
while (bytes_read < count)
|
|
{
|
|
rt_err_t result = rt_mq_recv(&servo_mq, &byte, sizeof(byte), timeout_ticks);
|
|
if (result == RT_EOK)
|
|
{
|
|
DBG_MB("rx: %02X\n", byte);
|
|
buf[bytes_read++] = byte;
|
|
}
|
|
else
|
|
{
|
|
if (bytes_read > 0)
|
|
{
|
|
DBG_MB("Modbus read finished %d\n", bytes_read);
|
|
return bytes_read;
|
|
}
|
|
DBG_MB("Servo read timeout, no data received\n");
|
|
return 0;
|
|
}
|
|
}
|
|
DBG_MB("Actually read:%d, count:%d\r\n", bytes_read, count);
|
|
|
|
return bytes_read;
|
|
}
|
|
|
|
int32_t bsp_servo_uart_write(const uint8_t *buf, uint16_t count, int32_t byte_timeout_ms, void *arg)
|
|
{
|
|
UNUSED(arg);
|
|
rt_thread_mdelay(2);
|
|
if (HAL_UART_Transmit(&SERVO_COM, buf, count, byte_timeout_ms) == HAL_OK)
|
|
{
|
|
return count;
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|