YuanHub 源码分析【五】速度模式举例通过 Mavlink 协议配置过

崔佳宇_

墨衍会员 · AI 创作全网分发码龄10年

关注

原创 2026-05-07 10:36:31 429 阅读 AI 写同款· GEO 优化 ›

STM32F0 USART 常用配置，DMA不定长接收，pingpong接收，硬件流控制，过采样，接收超时中断和空闲中断 STM32F0 USART 通信 1.DMA不定长接收 2.硬件流控制 3.过采样率 4. 阅读详情

一、UART2 的初始化

1.1 MX_USART2_UART_Init()

使用 CubeMX 初始化 uart2：

int main(void)
{

  ...
  MX_USART2_UART_Init();
  ...

}

static void MX_USART2_UART_Init(void)
{

  /* USER CODE BEGIN USART2_Init 0 */

  /* USER CODE END USART2_Init 0 */

  /* USER CODE BEGIN USART2_Init 1 */

  /* USER CODE END USART2_Init 1 */
  huart2.Instance = USART2;
  huart2.Init.BaudRate = 921600;
  huart2.Init.WordLength = UART_WORDLENGTH_8B;
  huart2.Init.StopBits = UART_STOPBITS_1;
  huart2.Init.Parity = UART_PARITY_NONE;
  huart2.Init.Mode = UART_MODE_TX_RX;
  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
  huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart2.Init.ClockPrescaler = UART_PRESCALER_DIV1;
  huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_RS485Ex_Init(&huart2, UART_DE_POLARITY_HIGH, 0, 0) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_SetTxFifoThreshold(&huart2, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_SetRxFifoThreshold(&huart2, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_UARTEx_DisableFifoMode(&huart2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART2_Init 2 */

  /* USER CODE END USART2_Init 2 */

}

分类	参数项目	配置值	功能说明
基础通信	波特率 (BaudRate)	921600	高速通信速率（921.6 kbps），适合传输大量或高频数据
	数据位宽 (WordLength)	8 Bits	每帧包含 8 位有效数据，最常用的标准配置
	停止位 (StopBits)	1 Bit	帧结尾使用 1 个停止位标识数据结束
	校验位 (Parity)	无校验 (None)	不添加奇偶校验位，节省带宽
	工作模式 (Mode)	收发使能 (TX_RX)	同时开启发送 (TX) 和接收 (RX) 功能
时钟与采样	时钟分频 (ClockPrescaler)	不分频 (DIV1)	串口内核外设时钟等于系统分配的基础时钟频率
	过采样率 (OverSampling)	16 倍 (16)	16倍过采样（相对8倍而言），对时钟偏差容忍度更高，接收更稳定
时钟与采样	单比特采样 (OneBitSampling)	禁用 (Disable)	采用多数投票机制（对中间3个采样点取多数）判定电平，抗噪能力更强
高级控制	硬件流控 (HwFlowCtl)	无 (None)	不使用 RTS/CTS 硬件流控引脚
RS-485模式	RS485 驱动使能 (DE)	开启	使用 `HAL_RS485Ex_Init`，通过硬件自动控制发送使能(DE)引脚方向
	DE 极性 (DE Polarity)	高电平 (High)	极性为高电平有效（发送时拉高 DE 引脚，接收时自动拉低）
FIFO 控制	FIFO 模式	禁用 (Disable)	关闭了硬件 FIFO 功能（注：代码中虽配置了1/8阈值，但紧接着被 `DisableFifoMode` 禁用了）

在别的地方为 uart2 开启了 DMA 中断：

    // 启动调试软件通讯
    __HAL_UART_CLEAR_IDLEFLAG(&HOST_UART_HANDLE);
    HAL_UART_Receive_DMA(&HOST_UART_HANDLE, mavlink_rx_buff, MAVLINK_RECV_BUFF_SIZE);
    __HAL_UART_ENABLE_IT(&HOST_UART_HANDLE, UART_IT_IDLE);

二、解析 Mavlink 协议

2.1 串口中断函数

uart2 宏定义：

#define HOST_UART_IRQ_TASK          USART2_IRQHandler

下面为 uart2 中断，没什么好说的：

/**
 * @brief 上位机通讯UART中断任务
 * 该函数用于处理UART中断事件，主要用于处理IDLE空闲中断
 * 当检测到UART空闲状态时，会停止DMA传输，处理接收到的数据，然后重新启动DMA接收
 */
void HOST_UART_IRQ_TASK(void)
{
    HAL_UART_IRQHandler(&HOST_UART_HANDLE); // 调用HAL库的UART中断处理函数

    if (__HAL_UART_GET_FLAG(&HOST_UART_HANDLE, UART_FLAG_IDLE))
    {
        __HAL_UART_CLEAR_IDLEFLAG(&HOST_UART_HANDLE);

        HAL_UART_DMAStop(&HOST_UART_HANDLE);

        // 调用Mavlink数据接收回调函数，处理接收到的数据
        // 参数为接收缓冲区和实际接收到的数据长度
#ifndef USE_CAN_MAVLINK_HOST
        MavlinkRecvCallback(&kAxis, &kAxisDw, mavlink_rx_buff, MAVLINK_RECV_BUFF_SIZE - __HAL_DMA_GET_COUNTER(HOST_UART_HANDLE.hdmarx));
#endif /* USE_CAN_MAVLINK_HOST */
        // 重新启动DMA接收，准备接收下一批数据
        HAL_UART_Receive_DMA(&HOST_UART_HANDLE, mavlink_rx_buff, MAVLINK_RECV_BUFF_SIZE);
    }

    // 及时检查溢出错误(ORE)，清除错误标志并重新启动DMA接收
    if (__HAL_UART_GET_FLAG(&HOST_UART_HANDLE, UART_FLAG_ORE))
    {
        __HAL_UART_CLEAR_FLAG(&HOST_UART_HANDLE, UART_FLAG_ORE);
        HAL_UART_DMAStop(&HOST_UART_HANDLE);
        HAL_UART_Receive_DMA(&HOST_UART_HANDLE, mavlink_rx_buff, MAVLINK_RECV_BUFF_SIZE);
    }
}

2.2 Mavlink 解析函数

进入 MavlinkRecvCallback 解析的状态机：

void MavlinkRecvCallback(Axis *axis, AxisDw *axis_dw, uint8_t rx_data[], uint32_t len)
{
    // 逐字节送入 MAVLink 状态机解析
    for (int i = 0; i < len; i++) {
        if (mavlink_parse_char(MAVLINK_COMM_0, rx_data[i], &msg, &status)) {
            mavlink_flag = 1;
            break;
        }
    }
    ...
}

mavlink_parse_char() 是 MAVLink 协议栈的标准函数，它维护一个内部状态机，逐字节处理：

当一帧完整消息解析成功后，返回成功，msg 结构体中包含了 msgid 和 payload。

if (msg.msgid == MAVLINK_MSG_ID_READ_PARAM) {
    // ★ 读请求：根据 struct_id 读取对应数据并回传
    mavlink_msg_read_param_decode(&msg, &read_param);
    switch (read_param.struct_id) {
        case MAVLINK_MSG_ID_AppControlWord:
            // 打包当前控制字回传
            ...
        case MAVLINK_MSG_ID_AppMotionParam:
            // 打包当前运动参数回传
            ...
    }
} else {
    // ★ 写请求：根据 msgid 解码数据并写入对应参数
    switch (msg.msgid) {
        case MAVLINK_MSG_ID_AppControlWord:   // 写控制字
        case MAVLINK_MSG_ID_AppOpMode:        // 写运行模式
        case MAVLINK_MSG_ID_AppMotionParam:   // 写运动参数
        case MAVLINK_MSG_ID_PosSpeedCtlConfig:// 写位置速度环配置
        ...
    }
}

三、速度模式的三个配置函数

3.1 概述

上位机通过 Mavlink 协议发送速度模式命令在 MavlinkRecvCallback() 中会触发三个

3.2 set_app_Modes_of_operation()

set_app_Modes_of_operation() 函数的作用是给系统切换各种模式，如从位置模式切换到速度模式。

在 MavlinkRecvCallback() 函数中这里调用：

case MAVLINK_MSG_ID_AppOpMode:
    mavlink_msg_appopmode_decode(&msg, (mavlink_appopmode_t *)&app_op_mode_t);
    if(get_app_Comm_control_authority() == COMM_CONTROL_HOST)    {set_app_Modes_of_operation(app_op_mode_t.Modes_of_operation);}
    app_op_mode_t.Modes_of_operation = get_app_Modes_of_operation();
    mavlink_msg_appopmode_encode(sys_id, comp_id, &send_msg, (mavlink_appopmode_t     *)&app_op_mode_t);
 break;

set_app_Modes_of_operation() 函数内部结构：

uint32_t set_app_Modes_of_operation(int8_t val)
{
    /* USER CODE BEGIN set_app_Modes_of_operation 0 */
    if (val == get_app_Modes_of_operation_display())
    {
        return APP_PARAM_SUCCESS;
    }
    /* USER CODE END set_app_Modes_of_operation 0 */
    kAppOpMode.Modes_of_operation = val;
    /* USER CODE BEGIN set_app_Modes_of_operation 1 */
    if (callback_registry.set_app_callback == NULL)
    {
        return APP_PARAM_OUT_OF_RANGE;
    }
    switch (kAppOpMode.Modes_of_operation)
    {
    case MOTOR_CTL_SM_MODE_POSITION:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_POSITION;
        callback_registry.set_app_callback(APP_PP_MODE);
        break;
    case MOTOR_CTL_SM_MODE_SPEED:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_SPEED;
        callback_registry.set_app_callback(APP_PV_MODE);
        break;
    case MOTOR_CTL_SM_MODE_TORQUE:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_TORQUE;
        callback_registry.set_app_callback(APP_PT_MODE);
        break;
    case MOTOR_CTL_SM_MODE_MIT:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_MIT;
        callback_registry.set_app_callback(APP_MIT_MODE);
        break;
    case MOTOR_CTL_SM_MODE_HOMING: // 回零模式实际使用速度模式
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_SPEED;
        callback_registry.set_app_callback(APP_HM_MODE);
        break;
    case MOTOR_CTL_SM_MODE_CSP:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_POSITION;
        callback_registry.set_app_callback(APP_CSP_MODE);
        break;
    case MOTOR_CTL_SM_MODE_CSV:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_SPEED;
        callback_registry.set_app_callback(APP_CSV_MODE);
        break;
    case MOTOR_CTL_SM_MODE_CST:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_TORQUE;
        callback_registry.set_app_callback(APP_CST_MODE);
        break;
    case MOTOR_CTL_SM_MODE_DIRECTION_IDENTIFICATION:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_DIRECTION_IDENTIFICATION;
        callback_registry.set_app_callback(APP_DIR_ID_MODE);
        break;
    case MOTOR_CTL_SM_MODE_ELECTRIC_ANGLE_IDENTIFICATION:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_ELECTRIC_ANGLE_IDENTIFICATION;
        callback_registry.set_app_callback(APP_ELEC_ANGLE_ID_MODE);
        break;
    case MOTOR_CTL_SM_MODE_ELECTRIC_IDENTIFICATION:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_ELECTRIC_IDENTIFICATION;
        callback_registry.set_app_callback(APP_ELEC_ID_MODE);
        break;
    case MOTOR_CTL_SM_MODE_MECHANICAL_IDENTIFICATION:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_MECHANICAL_IDENTIFICATION;
        callback_registry.set_app_callback(APP_MEC_ID_MODE);
        break;
    case MOTOR_CTL_SM_MODE_POLE_PAIRS_IDENTIFICATION:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_POLE_PAIRS_IDENTIFICATION;
        callback_registry.set_app_callback(APP_POLE_PAIRS_ID_MODE);
        break;
    case MOTOR_CTL_SM_MODE_TQ_FRICTION_IDENTIFICATION:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_TQ_FRICTION_IDENTIFICATION;
        callback_registry.set_app_callback(APP_TQ_FC_ID_MODE);
        break;
    default:
        axis->motor_ctl_sm_config.mode = MOTOR_CTL_SM_MODE_IDLE;
        break;
    }
    /* USER CODE END set_app_Modes_of_operation 1 */
    return APP_PARAM_SUCCESS;
}

3.3 set_app_Target_velocity()

set_app_Target_velocity() 函数的作用是设置并更新电机的目标速度值。它接收上位机 (通过 MAVLink) 发送过来的目标速度值。在 Profile Velocity (PV) 速度模式下，这个值代表了电机最终希望达到的稳定转速。

在 MavlinkRecvCallback() 函数中这里调用：

case MAVLINK_MSG_ID_AppMotionParam:
...
{set_app_Target_velocity(app_motion_param_t.Target_velocity);}
...
break;

set_app_Target_velocity() 函数内部结构：

uint32_t set_app_Target_velocity(float val)
{
    /* USER CODE BEGIN set_app_Target_velocity 0 */
    if (MATH_ABS(val) > kAppRestrictParam.Max_profile_velocity)
    {
        return APP_PARAM_OUT_OF_RANGE;
    }

    /* USER CODE END set_app_Target_velocity 0 */
    kAppMotionParam.Target_velocity = val;
    /* USER CODE BEGIN set_app_Target_velocity 1 */
    /* USER CODE END set_app_Target_velocity 1 */
    return APP_PARAM_SUCCESS;
}

3.4 set_app_Controlword()

set_app_Controlword() 是控制电机的函数。上位机的所有动作意图（启动、停止、复位、急停）都必须编码成这个 16 位的数值，通过此函数注入系统，进而影响 PDS_FSA_Run() 状态机的走向。

在 MavlinkRecvCallback() 函数中这里调用：

case MAVLINK_MSG_ID_AppControlWord:
    mavlink_msg_appcontrolword_decode(&msg, (mavlink_appcontrolword_t *)&app_control_word_t);
    if(get_app_Comm_control_authority() == COMM_CONTROL_HOST)    {set_app_Controlword(app_control_word_t.Controlword);}
    app_control_word_t.Controlword = get_app_Controlword();
    if(get_app_Comm_control_authority() == COMM_CONTROL_HOST)    {set_app_Halt_running_cmd(app_control_word_t.Halt_running_cmd);}
    app_control_word_t.Halt_running_cmd = get_app_Halt_running_cmd();
    mavlink_msg_appcontrolword_encode(sys_id, comp_id, &send_msg, (mavlink_appcontrolword_t *)&app_control_word_t);
break;

set_app_Controlword() 函数内部结构：

uint32_t set_app_Controlword(uint16_t val)
{
    if (val < 0)
        return APP_PARAM_OUT_OF_RANGE;
    if (val > 3)
        return APP_PARAM_OUT_OF_RANGE;

    /* USER CODE BEGIN set_app_Controlword 0 */
    // 切换到控制层控制后,应用层控制字不再生效
    if (get_app_Internal_control_authority() == INTERNAL_CONTROL_CTRL)
    {
        return APP_PARAM_READ_ONLY;
    }
    /* USER CODE END set_app_Controlword 0 */
    kAppControlWord.Controlword = val;
    /* USER CODE BEGIN set_app_Controlword 1 */
    axis->motor_ctl_sm_input.motor_enable = val;
    /* USER CODE END set_app_Controlword 1 */
    return APP_PARAM_SUCCESS;
}

收录于