537 lines
13 KiB
C
537 lines
13 KiB
C
#include "board.h"
|
||
#include "motor.h"
|
||
|
||
extern uint8_t detect_result;
|
||
void motor_process(frame_msg_t *rx, frame_msg_t *bk);
|
||
void sensor_process(frame_msg_t *rx, frame_msg_t *bk);
|
||
void error_process(uint8_t err, frame_msg_t *rx, frame_msg_t *bk);
|
||
|
||
/************************************* 串口通讯 *************************************/
|
||
uart_t *uarts[UART_NUM_MAX];
|
||
__IO static BOOL execute_state = IDLE; // 数据处理标志
|
||
static frame_msg_t rx_msg; // 接收数据句柄
|
||
static frame_msg_t bk_msg; // 回复数据句柄
|
||
static uint8_t host_send_buffer[UART_TXSIZE] = {0}; // 数据发送缓存
|
||
|
||
// 串口发送
|
||
static void host_send_msg(uint8_t *data, uint16_t len)
|
||
{
|
||
uart_send_data(uarts[UART_NUM_1], data, len);
|
||
}
|
||
|
||
// 数据有效性检查
|
||
static BOOL host_data_verify(uint8_t *data, uint16_t len)
|
||
{
|
||
BOOL ret = TRUE;
|
||
uint8_t ver_len = 0;
|
||
uint8_t msg_crc = 0, ver_crc = 0;
|
||
|
||
// 包头包尾检查
|
||
if (data[0] != PACKET_STX || data[1] != ST_DEV_NORMAL || data[len - 1] != PACKET_ETX)
|
||
{
|
||
ret = FALSE;
|
||
return ret;
|
||
}
|
||
// 长度检查
|
||
ver_len = PACKET_MIN_LEN + data[DATA_LEN_INDEX];
|
||
if (ver_len != len)
|
||
{
|
||
ret = FALSE;
|
||
return ret;
|
||
}
|
||
// CRC校验
|
||
msg_crc = data[len - 2];
|
||
ver_crc = xor_compute((uint8_t *)&data[1], (len - 3)); // 减去包头、包尾、校验
|
||
if (msg_crc != ver_crc)
|
||
{
|
||
ret = FALSE;
|
||
return ret;
|
||
}
|
||
|
||
return ret;
|
||
}
|
||
|
||
// 数据解码
|
||
static void host_data_decode(uint8_t *data, uint16_t len)
|
||
{
|
||
DBG_ASSERT(data != NULL __DBG_LINE);
|
||
|
||
// 获取数据长度和消息长度
|
||
uint8_t data_len = data[DATA_LEN_INDEX];
|
||
uint8_t msg_len = DATA_INDEX + data_len;
|
||
// 获取消息数据
|
||
osel_memcpy((uint8_t *)&rx_msg, data, msg_len);
|
||
}
|
||
|
||
// 指令执行
|
||
static void host_cmd_execute(frame_msg_t *rx_msg, frame_msg_t *bk_msg)
|
||
{
|
||
DBG_ASSERT(rx_msg != NULL __DBG_LINE);
|
||
DBG_ASSERT(bk_msg != NULL __DBG_LINE);
|
||
|
||
switch (rx_msg->dev_no)
|
||
{
|
||
case ADC_SENSOR:
|
||
sensor_process(rx_msg, bk_msg);
|
||
break;
|
||
case MOTOR:
|
||
motor_process(rx_msg, bk_msg);
|
||
break;
|
||
default:
|
||
error_process(ST_CMD_IVALID, rx_msg, bk_msg);
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 数据编码
|
||
static void host_data_encode(frame_msg_t *msg)
|
||
{
|
||
DBG_ASSERT(msg != NULL __DBG_LINE);
|
||
|
||
uint8_t *pbuf = (uint8_t *)host_send_buffer;
|
||
uint8_t index = 0, len = 0;
|
||
|
||
// 消息数据
|
||
uint8_t msg_len = DATA_INDEX + msg->len;
|
||
osel_memcpy(pbuf, (uint8_t *)msg, msg_len);
|
||
// 校验码
|
||
index += msg_len;
|
||
pbuf[index] = xor_compute((uint8_t *)&pbuf[1], (msg_len - 1)); // 减去包头
|
||
// 包尾
|
||
index += 1;
|
||
pbuf[index] = PACKET_ETX;
|
||
|
||
// 计算报文长度
|
||
len = msg_len + 2; // 加上校验、包尾
|
||
// 发送
|
||
host_send_msg(pbuf, len);
|
||
}
|
||
|
||
// 数据处理
|
||
void host_rx_msg_deal(void)
|
||
{
|
||
if (execute_state == IDLE)
|
||
{
|
||
return;
|
||
}
|
||
|
||
frame_msg_t *p_rx_msg = (frame_msg_t *)&rx_msg;
|
||
frame_msg_t *p_bk_msg = (frame_msg_t *)&bk_msg;
|
||
|
||
// 执行
|
||
host_cmd_execute(p_rx_msg, p_bk_msg);
|
||
// 回复（组包 + 发送）
|
||
host_data_encode(p_bk_msg);
|
||
// 复位
|
||
osel_memset((uint8_t *)host_send_buffer, 0, UART_TXSIZE);
|
||
osel_memset((uint8_t *)&rx_msg, 0, sizeof(frame_msg_t));
|
||
osel_memset((uint8_t *)&bk_msg, 0, sizeof(frame_msg_t));
|
||
execute_state = IDLE;
|
||
detect_result = ST_DEV_NORMAL;
|
||
}
|
||
|
||
// 串口接收回调
|
||
static void host_rx_cb(uint8_t uart_index, uint8_t *data, uint16_t len)
|
||
{
|
||
BOOL ret = FALSE;
|
||
|
||
// 数据有效性检查
|
||
ret = host_data_verify(data, len);
|
||
// 数据解码
|
||
if (ret == TRUE && execute_state == IDLE)
|
||
{
|
||
host_data_decode(data, len);
|
||
execute_state = BUSY;
|
||
}
|
||
}
|
||
|
||
// 串口初始化
|
||
static void host_uart_init(void)
|
||
{
|
||
if (uarts[UART_NUM_1] == NULL)
|
||
{
|
||
uarts[UART_NUM_1] = uart_create(USART1, TRUE, UART_RXSIZE, host_rx_cb, TRUE, UART_TXSIZE, NULL);
|
||
uarts[UART_NUM_1]->uart_index = UART_NUM_1;
|
||
uarts[UART_NUM_1]->dma = DMA1;
|
||
uarts[UART_NUM_1]->dma_rx_channel = LL_DMA_CHANNEL_3;
|
||
uarts[UART_NUM_1]->dma_tx_channel = LL_DMA_CHANNEL_2;
|
||
uart_recv_en(uarts[UART_NUM_1]);
|
||
}
|
||
}
|
||
|
||
/************************************* 电机 *************************************/
|
||
// 电机对象
|
||
motor_t *motor;
|
||
motor_ctrl_t *motor_param = NULL;
|
||
|
||
// 电机初始化
|
||
static void motor_init(void)
|
||
{
|
||
motor = motor_create(STEP_MOTOR);
|
||
gpio_t dir = {
|
||
.port = DIR_GPIO_Port,
|
||
.pin = DIR_Pin,
|
||
};
|
||
gpio_t en = {
|
||
.port = ENA_GPIO_Port,
|
||
.pin = ENA_Pin,
|
||
};
|
||
motor->handle.step_motor.interface.init(motor, dir, en, MIN_STEP_ANGLE, TIM21, LL_TIM_CHANNEL_CH2);
|
||
}
|
||
|
||
static uint8_t motor_param_get(frame_msg_t *rx, motor_ctrl_t *param)
|
||
{
|
||
uint8_t state = ST_DEV_NORMAL;
|
||
|
||
// 检查数据长度，方向：1个字节，角度：4个字节
|
||
if (rx->len < 5)
|
||
{
|
||
state = ST_CMD_IVALID;
|
||
return state;
|
||
}
|
||
// 方向
|
||
param->dir = rx->data[0];
|
||
// 角度
|
||
osel_memcpy((uint8_t *)¶m->angle, (uint8_t *)&rx->data[1], 4);
|
||
param->angle = B2S_FLOAT32(param->angle);
|
||
|
||
return state;
|
||
}
|
||
|
||
static uint8_t motor_ctrl(uint8_t sw, motor_ctrl_t *param)
|
||
{
|
||
step_motor_t *step_motor = &motor->handle.step_motor;
|
||
uint8_t state = detect_result;
|
||
if (sw == RUN && detect_result == ST_DEV_NORMAL)
|
||
{
|
||
step_motor->interface.set_angle(motor, param->angle, (dir_e)param->dir);
|
||
}
|
||
else
|
||
{
|
||
step_motor->interface.stop(motor);
|
||
}
|
||
return state;
|
||
}
|
||
|
||
static void motor_state(frame_msg_t *rx, frame_msg_t *bk)
|
||
{
|
||
DBG_ASSERT(bk != NULL __DBG_LINE);
|
||
|
||
step_motor_t *step_motor = &motor->handle.step_motor;
|
||
uint8_t state = ST_DEV_NORMAL;
|
||
|
||
// 回复数据
|
||
bk->head = PACKET_STX;
|
||
bk->state = state;
|
||
bk->dev_no = rx->dev_no;
|
||
bk->cmd_no = rx->cmd_no;
|
||
bk->len = sizeof(uint8_t);
|
||
bk->data[0] = step_motor->attribute.state;
|
||
}
|
||
|
||
void motor_process_init(void)
|
||
{
|
||
motor_param = (motor_ctrl_t *)osel_mem_alloc(sizeof(motor_ctrl_t));
|
||
DBG_ASSERT(motor_param != NULL __DBG_LINE);
|
||
osel_memset((uint8_t *)motor_param, 0, sizeof(motor_ctrl_t));
|
||
}
|
||
|
||
// 电机任务
|
||
void motor_process(frame_msg_t *rx, frame_msg_t *bk)
|
||
{
|
||
DBG_ASSERT(rx != NULL __DBG_LINE);
|
||
DBG_ASSERT(bk != NULL __DBG_LINE);
|
||
|
||
motor_ctrl_t *ctrl_param = motor_param;
|
||
uint8_t state = ST_DEV_NORMAL;
|
||
|
||
switch (rx->cmd_no)
|
||
{
|
||
case SET_MOTOR_SPEED: // 设置电机转速
|
||
break;
|
||
case MOTOR_MOVE: // 运行电机
|
||
{
|
||
// 读取参数
|
||
state = motor_param_get(rx, ctrl_param);
|
||
// 运行电机
|
||
if (state == ST_DEV_NORMAL)
|
||
{
|
||
state = motor_ctrl(RUN, ctrl_param);
|
||
}
|
||
// 回复数据
|
||
osel_memcpy((uint8_t *)bk, (uint8_t *)rx, sizeof(frame_msg_t));
|
||
bk->state = state;
|
||
break;
|
||
}
|
||
case MOTOR_STOP: // 停止电机
|
||
{
|
||
// 停止电机
|
||
state = motor_ctrl(STOP, ctrl_param);
|
||
// 回复数据
|
||
osel_memcpy((uint8_t *)bk, (uint8_t *)rx, sizeof(frame_msg_t));
|
||
bk->state = state;
|
||
break;
|
||
}
|
||
case MOTOR_STATE:
|
||
motor_state(rx, bk);
|
||
break;
|
||
default:
|
||
error_process(ST_CMD_IVALID, rx, bk);
|
||
break;
|
||
}
|
||
}
|
||
|
||
/************************************* ADc传感器 *************************************/
|
||
uint8_t detect_result = ST_DEV_NORMAL; // 数据转换结果
|
||
uint16_t sensor_adc[GET_ALL_VALUE] = {0};
|
||
|
||
// AD传感器
|
||
adc_t adc1 = {
|
||
.adc = ADC1,
|
||
.dma = DMA1,
|
||
.dma_channel = LL_DMA_CHANNEL_1,
|
||
};
|
||
|
||
uint16_t get_sensor_adc(uint8_t id)
|
||
{
|
||
uint16_t adc = 0;
|
||
|
||
switch (id)
|
||
{
|
||
case GET_TORSION_VALUE: // 扭力，单位：N·m
|
||
adc = adc_get_result_average(IN13);
|
||
break;
|
||
case GET_INPUT_PRESS_VALUE: // 输入压力，单位：kPa
|
||
adc = adc_get_result_average(IN7);
|
||
break;
|
||
case GET_OUTPUT_PRESS_VALUE: // 输出压力，单位：kPa
|
||
adc = adc_get_result_average(IN9);
|
||
break;
|
||
case GET_INPUT_FLOW_VALUE: // 输入流量，单位：SLPM
|
||
adc = adc_get_result_average(IN8);
|
||
break;
|
||
case GET_OUTPUT_FLOW_VALUE: // 输出流量，单位：SLPM
|
||
adc = adc_get_result_average(IN10);
|
||
break;
|
||
}
|
||
|
||
return adc;
|
||
}
|
||
|
||
float32 torsion_detect(void)
|
||
{
|
||
float32 adc = 0.0, val = 0.0;
|
||
|
||
adc = get_sensor_adc(GET_TORSION_VALUE);
|
||
val = adc / 4096 * 3000;
|
||
if (val < 914 || val > 2086)
|
||
{
|
||
detect_result = ST_DEV_MISS;
|
||
}
|
||
val = (val - 1500) / 101 / (1.934 * 3) * 10;
|
||
|
||
return val;
|
||
}
|
||
|
||
float32 pressure_detect(uint8_t id)
|
||
{
|
||
float32 adc = 0.0, val = 0.0;
|
||
|
||
adc = get_sensor_adc(id);
|
||
val = adc / 4096 * 3000;
|
||
if (val < 500 || val > 3000)
|
||
{
|
||
detect_result = ST_DEV_MISS;
|
||
}
|
||
val = (val - 600) / (10 * 15) / 16 * 1600;
|
||
|
||
return val;
|
||
}
|
||
|
||
float32 flow_detect(uint8_t id)
|
||
{
|
||
float32 adc = 0.0, val = 0.0;
|
||
|
||
adc = get_sensor_adc(id);
|
||
val = adc / 4096 * 3000;
|
||
if (val < 500 || val > 3000)
|
||
{
|
||
detect_result = ST_DEV_MISS;
|
||
}
|
||
val = (val - 600) / (10 * 15) / 16 * 300;
|
||
|
||
return val;
|
||
}
|
||
|
||
// 传感器任务
|
||
void sensor_process(frame_msg_t *rx, frame_msg_t *bk)
|
||
{
|
||
DBG_ASSERT(rx != NULL __DBG_LINE);
|
||
DBG_ASSERT(bk != NULL __DBG_LINE);
|
||
|
||
uint8_t state = ST_DEV_NORMAL;
|
||
|
||
// 检查指令合法性
|
||
if (rx->cmd_no > GET_ALL_VALUE)
|
||
{
|
||
state = ST_CMD_IVALID;
|
||
goto encode;
|
||
}
|
||
|
||
encode:
|
||
// 回复数据
|
||
bk->head = PACKET_STX;
|
||
bk->state = state;
|
||
bk->dev_no = rx->dev_no;
|
||
bk->cmd_no = rx->cmd_no;
|
||
if (rx->cmd_no >= GET_ALL_VALUE)
|
||
{
|
||
bk->len = sizeof(uint16_t) * GET_ALL_VALUE;
|
||
osel_memcpy((uint8_t *)bk->data, (uint8_t *)&sensor_adc, bk->len);
|
||
}
|
||
else
|
||
{
|
||
bk->len = sizeof(uint16_t);
|
||
osel_memcpy((uint8_t *)bk->data, (uint8_t *)&sensor_adc[bk->cmd_no], bk->len);
|
||
}
|
||
}
|
||
|
||
/************************************* 测试 *************************************/
|
||
#if SELF_TEST
|
||
test_t self_test = {TEST_DEV_IDEL, 0};
|
||
uint8_t test_buffer[UART_RXSIZE] = {0}; // 数据接收缓存
|
||
#endif
|
||
|
||
// 传感器测试指令
|
||
static uint8_t test_sensor_cmd(uint8_t cmd, uint8_t *buf)
|
||
{
|
||
osel_memset(buf, 0, UART_RXSIZE);
|
||
|
||
uint8_t ver_len = 0;
|
||
uint8_t index = 0;
|
||
frame_msg_t msg;
|
||
msg.head = PACKET_STX;
|
||
msg.state = ST_DEV_NORMAL;
|
||
msg.dev_no = ADC_SENSOR;
|
||
msg.cmd_no = cmd;
|
||
msg.len = 0;
|
||
index = DATA_INDEX;
|
||
osel_memcpy(buf, (uint8_t *)&msg, index);
|
||
// 校验
|
||
buf[index] = xor_compute((uint8_t *)&buf[1], (index - 1)); // 减掉包头
|
||
index += 1;
|
||
// 包尾
|
||
buf[index] = PACKET_ETX;
|
||
index += 1;
|
||
// 帧长度
|
||
ver_len = index;
|
||
return ver_len;
|
||
}
|
||
|
||
// 电机测试指令
|
||
static uint8_t test_motor_cmd(uint8_t cmd, uint8_t *buf)
|
||
{
|
||
uint8_t ver_len = 0;
|
||
uint8_t index = 0;
|
||
frame_msg_t msg;
|
||
motor_ctrl_t motor_param;
|
||
msg.head = PACKET_STX;
|
||
msg.state = ST_DEV_NORMAL;
|
||
msg.dev_no = MOTOR;
|
||
msg.cmd_no = cmd;
|
||
switch (cmd)
|
||
{
|
||
case MOTOR_MOVE:
|
||
msg.len = 5; // 方向1个字节，角度4个字节
|
||
break;
|
||
case MOTOR_STOP:
|
||
msg.len = 0;
|
||
break;
|
||
}
|
||
index = DATA_INDEX;
|
||
osel_memcpy(buf, (uint8_t *)&msg, index);
|
||
|
||
if (cmd == MOTOR_MOVE)
|
||
{
|
||
motor_param.dir = DIR_CCW;
|
||
motor_param.angle = 90;
|
||
motor_param.angle = S2B_FLOAT32(motor_param.angle);
|
||
}
|
||
osel_memcpy((uint8_t *)&buf[index], (uint8_t *)&motor_param, sizeof(motor_ctrl_t));
|
||
index += sizeof(motor_ctrl_t);
|
||
// 校验
|
||
buf[index] = xor_compute((uint8_t *)&buf[1], (index - 1)); // 减掉包头
|
||
index += 1;
|
||
// 包尾
|
||
buf[index] = PACKET_ETX;
|
||
index += 1;
|
||
// 帧长度
|
||
ver_len = index;
|
||
return ver_len;
|
||
}
|
||
|
||
// 发送测试数据
|
||
static void test_rx_cb(uint8_t *data, uint8_t len)
|
||
{
|
||
BOOL ret = FALSE;
|
||
|
||
// 数据有效性检查
|
||
ret = host_data_verify(data, len);
|
||
// 数据解码
|
||
if (ret == TRUE && execute_state == IDLE)
|
||
{
|
||
host_data_decode(data, len);
|
||
execute_state = BUSY;
|
||
}
|
||
}
|
||
|
||
// 测试任务
|
||
void test_process(void)
|
||
{
|
||
uint8_t len = 0;
|
||
uint8_t *buf = test_buffer;
|
||
|
||
if (self_test.dev_id >= TEST_DEV_MAX)
|
||
{
|
||
return;
|
||
}
|
||
|
||
switch (self_test.dev_id)
|
||
{
|
||
case TEST_DEV_SENSOR:
|
||
len = test_sensor_cmd(self_test.cmd_id, buf);
|
||
break;
|
||
case TEST_DEV_MOTOR:
|
||
len = test_motor_cmd(self_test.cmd_id, buf);
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
|
||
test_rx_cb(buf, len);
|
||
self_test.dev_id = TEST_DEV_IDEL;
|
||
}
|
||
|
||
/************************************* 板卡 *************************************/
|
||
// 板卡初始化
|
||
void board_init(void)
|
||
{
|
||
motor = NULL;
|
||
my_mem_init(SRAMIN);
|
||
|
||
motor_init(); // 电机初始化
|
||
adc_init(adc1); // 初始化ADC1通道，默认采集AD
|
||
host_uart_init(); // 串口初始化
|
||
ENABLE_TIM(TIM6); // 任务流程定时器使能
|
||
}
|
||
|
||
// 不支持的任务
|
||
void error_process(uint8_t err, frame_msg_t *rx, frame_msg_t *bk)
|
||
{
|
||
DBG_ASSERT(rx != NULL __DBG_LINE);
|
||
DBG_ASSERT(bk != NULL __DBG_LINE);
|
||
|
||
osel_memcpy((uint8_t *)bk, (uint8_t *)rx, sizeof(frame_msg_t));
|
||
bk->state = err;
|
||
}
|