前言

在上下位机的的通讯中，如果遇到需要传输具有复杂格式的数据比如激光雷达数据，imu数据，里程计数据等传感器数据的场景，这时自己编写通讯协议往往十分复杂与耗时。因此这个时候可以使用一些现成的通讯协议来进行传输。MAVLink就是一种非常方便的多平台通讯协议。本文以传输imu数据为例，讲解上下位机如何使用MAVLink协议。

 

目录

前言

一、下位机（STM32F407ZGT6）MAVLIink配置流程

1.1 硬件准备

1.2 软件准备

1.3 驱动安装

1.4 代码编写

1.5 上机测试

1.6 代码

 二 、上位机（树莓派5）配置流程

2.1 环境配置

2.2 创建ros2功能包

2.3 启动脚本

2.4 测试：创建一个订阅者读取imu数据

2.5 代码

 

 

 

 

一、下位机（STM32F407ZGT6）MAVLIink配置流程

1.1 硬件准备

  1. 开发板：stm32F407zgt6（使用的开发板为轮趣科技公司的C30-2.0D）

  2. 数据线：USB转Type-c数据线

1.2 软件准备

  1. keil5软件安装，要求下载F407系列资源包，可以正常配置407的环境
  2. stm32cubemx安装，本系列所写代码为stm32hal库，运用stm32cubemx可以快速编写基本的所需环境，具体教学可以上b站大学学习，学103系列的hal也可使用，区别在于103系列与407系列的引脚数量与申明不同（ps：有能力的可以使用vscode等其他软件的功能编写）

1.3 驱动安装

  1. 该代码运用串口1进行数据通讯，也可以使用蓝牙等方法进行通讯，在这不做展示，由于该开发板使用的usb转ttl芯片为ch9102，所以需要提前下载相关驱动包，CH340以及CH系列USB转串口驱动下载_ch9102驱动下载-CSDN博客（注意上位机要安装相关驱动包，此处是以电脑作为上位机，如果以树莓派作为上位机需要安装对应驱动包）
  2. mavlink库的安装有两种方法
    1.如果使用的寻常变量包含在mavlink库中的话，就可以直接去官网里的链接跳转GitHub中下载标准库使用 C MAVLink 库 (mavgen) | MAVLink Guide
    2.如果需要自定义变量的话需要用mavgen进行生成，需要注意的是要想使用mavgen需要提前下载python环境。（不过同样的，标准库的mavlink也可以直接用mavgen生成
    MAVLink 开发人员指南 | MAVLink Guide

1.4 代码编写

  1. 使用stm32cubemx生成需要的基本底层代码
    1.配置RCC与SYS  ​编辑 
      ​编辑 
    （黄色警告不影响程序，只是由于部分硬件占用导致部分选项无法选择导致的警告）
    2.配置通讯串口
      ​编辑 
    （可以在右侧面板摁住ctrl来选择串口所使用的引脚）
    3.配置CLK
      ​编辑 
    （大致配置内容为选择HCLK为最高频率，具体参数可以在代码内修改）
    4.选择版本与生成
      ​编辑 
      ​编辑 
    1. 来到keil5中打开文件
      1.将生成好的mavlink库放入keil5文件中，添加文件路径，这时候然后过编译会有很多报错，可以先不管，报错的原因主要是没有mavlink.c以及环境问题
      2.首先解决环境问题
      点击keil5中的魔术棒，切换到c/c++，修改Misc Controls的内容为
                                                  --c99 --no_strict --gnu
        ​编辑 
      3.接着在加入mavlink.c文件，需要自己编写 
      /*******************************************************************************
* 文件名称：mavlink_main.c
*
* 摘    要：mavlink自定义文件
*
* 当前版本：
* 作    者：
* 日    期：2018/05/30
* 编译环境：keil5
*
* 历史信息：
*******************************************************************************/
#include "main.h"
#include "mavlink.h"
#include "mavlink_types.h"
#include "bsp_icm20948.h"
#include "md_struct.h"
#include "ringbuffer.h"
#include "stdio.h"
extern UART_HandleTypeDef huart1;
extern IMU_DATA_t imu_data;
extern ATTITUDE_DATA_t imu_attitude;
#define  MAVLINK_READ_BUFFER_SIZE 128

/*缓冲区管理器*/
//ringbuffer管理变量
RingBuffer  m_Mavlink_RX_RingBuffMgr;
/*MAVLINK接收数据缓冲区数组*/
uint8_t   m_Mavlink_RX_Buff[MAVLINK_READ_BUFFER_SIZE*10];
uint8_t mavlink_byte;
/*******************************函数声明****************************************
* 函数名称: void Mavlink_RB_Init(void)
* 输入参数:
* 返回参数:
* 功    能: 初始化一个循环队列，用来管理接收到的串口数据。其实就是一个数据缓冲区
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
void Mavlink_RB_Init(void)
{
	//将m_Mavlink_RX_Buffm_Mavlink_RX_RingBuffMgr环队列进行关联管理。
	rbInit(&m_Mavlink_RX_RingBuffMgr, m_Mavlink_RX_Buff, sizeof(m_Mavlink_RX_Buff));
}

/*******************************函数声明****************************************
* 函数名称: void Mavlink_RB_Clear(void)
* 输入参数:
* 返回参数:
* 功    能: 归零ringbuffer里面的设置。
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
void Mavlink_RB_Clear(void)
{
	rbClear(&m_Mavlink_RX_RingBuffMgr);
}

/*******************************函数声明****************************************
* 函数名称: uint8_t  Mavlink_RB_IsOverFlow(void)
* 输入参数:
* 返回参数:  溢出为1，反之为0
* 功    能: 判断缓冲器是否溢出
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
uint8_t  Mavlink_RB_IsOverFlow(void)
{
	return m_Mavlink_RX_RingBuffMgr.flagOverflow;
}

/*******************************函数声明****************************************
* 函数名称: void Mavlink_RB_Push(uint8_t data)
* 输入参数: data：待压入的数据
* 返回参数:
* 功    能: 将接收的数据压入缓冲区
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
void Mavlink_RB_Push(uint8_t data)
{
	rbPush(&m_Mavlink_RX_RingBuffMgr, (uint8_t)(data & (uint8_t)0xFFU));
}

/*******************************函数声明****************************************
* 函数名称: uint8_t Mavlink_RB_Pop(void)
* 输入参数:
* 返回参数: uint8_t 读出的数据
* 功    能: 从缓冲器读出数据
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
uint8_t Mavlink_RB_Pop(void)
{
	return rbPop(&m_Mavlink_RX_RingBuffMgr);
}

/*******************************函数声明****************************************
* 函数名称: uint8_t Mavlink_RB_HasNew(void)
* 输入参数:
* 返回参数:
* 功    能: 判断是否有新的数据
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
uint8_t Mavlink_RB_HasNew(void)
{
	return !rbIsEmpty(&m_Mavlink_RX_RingBuffMgr);
}

/*******************************函数声明****************************************
* 函数名称: uint16_t Mavlink_RB_Count(void)
* 输入参数:
* 返回参数:
* 功    能: 判断有多少个新数据
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
uint16_t Mavlink_RB_Count(void)
{
	return rbGetCount(&m_Mavlink_RX_RingBuffMgr);
}

/*******************************函数声明****************************************
* 函数名称: void Mavlink_Rece_Enable(void)
* 输入参数:
* 返回参数:
* 功    能: 使能DMA接收
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
void Mavlink_Rece_Enable(void)
{
	HAL_UART_Receive_DMA(&huart1, &mavlink_byte, 1);
}

/*******************************函数声明****************************************
* 函数名称: void Mavlink_Init(void)
* 输入参数:
* 返回参数:
* 功    能: 初始化MAVLINK：使能接收，ringbuffer关联
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
void Mavlink_Init(void)
{
	Mavlink_Rece_Enable();
	Mavlink_RB_Init();
}

/*******************************函数声明****************************************
* 函数名称: void Mavlin_RX_InterruptHandler(void)
* 输入参数:
* 返回参数:
* 功    能: 串口中断的处理函数，主要是讲数据压入ringbuffer管理器
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
void Mavlin_RX_InterruptHandler(void)
{
	//数据压入
	rbPush(&m_Mavlink_RX_RingBuffMgr,mavlink_byte);
}
/*在“mavlink_helpers.h中需要使用”*/
mavlink_system_t mavlink_system =
{
	1,
	1
}; // System ID, 1-255, Component/Subsystem ID, 1-255

/*******************************函数声明****************************************
* 函数名称: void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
* 输入参数:
* 返回参数:
* 功    能: 重新定义mavlink的发送函数，与底层硬件接口关联起来
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
{
	HAL_UART_Transmit(&huart1, (uint8_t *)ch, length, 2000);
}

/*******************************函数声明****************************************
* 函数名称: void Mavlink_Msg_Handle(void)
* 输入参数:
* 返回参数:
* 功    能: 处理从QGC上位机传过来的数据信息
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
void Mavlink_Msg_Handle(mavlink_message_t msg)
{
	switch(msg.msgid)
	{
	case MAVLINK_MSG_ID_HEARTBEAT:
		printf("this is heartbeat from QGC/r/n");
		break;
	case MAVLINK_MSG_ID_SYS_STATUS:
//		  osd_vbat = (mavlink_msg_sys_status_get_voltage_battery(&msg) / 1000.0f); //Battery voltage, in millivolts (1 = 1 millivolt)
//			osd_curr = mavlink_msg_sys_status_get_current_battery(&msg); //Battery current, in 10*milliamperes (1 = 10 milliampere)
//			osd_battery_remaining = mavlink_msg_sys_status_get_battery_remaining(&msg); //Remaining battery energy: (0%: 0, 100%: 100)
		break;
	default:
		break;
	}
}
/*******************************函数声明****************************************
* 函数名称: Loop_Mavlink_Parse(void)
* 输入参数:
* 返回参数:
* 功    能: 在main函数中不间断调用此函数
* 作    者: by Across
* 日    期: 2018/06/02
*******************************************************************************/
mavlink_message_t msg;
mavlink_status_t status;

void Loop_Mavlink_Parse(void)
{
	if(Mavlink_RB_IsOverFlow())
	{
		Mavlink_RB_Clear();
	}

	while(Mavlink_RB_HasNew())
	{
		uint8_t read = Mavlink_RB_Pop();
		if(mavlink_parse_char(MAVLINK_COMM_0, read, &msg, &status))
		{
			//信号处理函数
			Mavlink_Msg_Handle(msg);
			//printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
		}
	}
}


///*测试函数代码区******************************************************************************/
static void mavlink_test_heartbeat2(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
    mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0);
    if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_HEARTBEAT >= 256)
    {
        return;
    }
#endif
    mavlink_heartbeat_t packet_in =
    {
        963497464,17,84,151,218,3
    };
    mavlink_heartbeat_t packet1, packet2;
    memset(&packet1, 0, sizeof(packet1));
    packet1.custom_mode = packet_in.custom_mode;
    packet1.type = packet_in.type;
    packet1.autopilot = packet_in.autopilot;
    packet1.base_mode = packet_in.base_mode;
    packet1.system_status = packet_in.system_status;
    packet1.mavlink_version = packet_in.mavlink_version;

#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
    if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)
    {
        memset(MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN);
    }
#endif
    memset(&packet2, 0, sizeof(packet2));
    mavlink_msg_heartbeat_send(MAVLINK_COMM_0 , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status );
    MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}

static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
    mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0);
    if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_HIGHRES_IMU >= 256)
    {
        return;
    }
#endif
    // 使用高分辨率IMU消息结构体
    mavlink_highres_imu_t packet1;
    memset(&packet1, 0, sizeof(packet1));
    
    // 设置时间戳
    packet1.time_usec = HAL_GetTick() * 1000; // 使用当前时间作为时间戳
    
    // 设置加速度计数据（浮点数）
    packet1.xacc = imu_data.accel.x;    // 加速度计 X 轴数据
    packet1.yacc = imu_data.accel.y;    // 加速度计 Y 轴数据
    packet1.zacc = imu_data.accel.z;    // 加速度计 Z 轴数据
    
    // 设置陀螺仪数据（浮点数）
    packet1.xgyro = imu_data.gyro.x;    // 陀螺仪 X 轴数据
    packet1.ygyro = imu_data.gyro.y;    // 陀螺仪 Y 轴数据
    packet1.zgyro = imu_data.gyro.z;    // 陀螺仪 Z 轴数据
    
    // 设置磁力计数据（浮点数）
    packet1.xmag = imu_attitude.pitch;  // 磁力计 X 轴数据
    packet1.ymag = imu_attitude.roll;   // 磁力计 Y 轴数据
    packet1.zmag = imu_attitude.yaw;    // 磁力计 Z 轴数据
    
    // 不需要的字段设为0
    packet1.abs_pressure = 0.0f;
    packet1.diff_pressure = 0.0f;
    packet1.pressure_alt = 0.0f;
    packet1.temperature = 0.0f;
    
    // 设置字段更新标志（只标记我们实际使用的字段）
    packet1.fields_updated = ((1 << 0) | (1 << 1) | (1 << 2) |  // 加速度计 (xacc, yacc, zacc)
                             (1 << 3) | (1 << 4) | (1 << 5) |  // 陀螺仪 (xgyro, ygyro, zgyro)
                             (1 << 6) | (1 << 7) | (1 << 8));  // 磁力计 (xmag, ymag, zmag)

#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1
    if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)
    {
        memset(MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN);
    }
#endif

    // 发送高分辨率IMU消息
    mavlink_msg_highres_imu_send(MAVLINK_COMM_0,
                                packet1.time_usec,
                                packet1.xacc, packet1.yacc, packet1.zacc,
                                packet1.xgyro, packet1.ygyro, packet1.zgyro,
                                packet1.xmag, packet1.ymag, packet1.zmag,
                                packet1.abs_pressure, packet1.diff_pressure,
                                packet1.pressure_alt, packet1.temperature,
                                packet1.fields_updated);
}

void mavlink_test(void)
{
    static uint16_t test_count = 0;
    mavlink_message_t lastmsg;
    test_count++;
    
    // 5Hz心跳包
    if((test_count % 100) == 0)
    {
        mavlink_test_heartbeat2(1, 1, &lastmsg);
    }
    
    // 10Hz高分辨率IMU数据
    if((test_count % 50) == 0)
    {
        mavlink_test_highres_imu(1, 1, &lastmsg);
    }
}

      3.mavlink缺失的文件在ringbuffer以及dma中（需要的可以去源文件中下载）
       

      #include "ringbuffer.h"
/**
  * @brief  rbInitialize初始化配置，将缓冲区信息填入结构体
  * @param  pRingBuff:ringbuffer结构体
	*         buff：数据缓冲区
	*         length：缓冲区大小
  * @note   
  * @retval void
  * @author Acorss工作室	
  */	
void rbInit(RingBuffer* pRingBuff, uint8_t* buff, uint16_t length)
{
	pRingBuff->pBuff = buff;
	pRingBuff->pEnd  = buff + length;
	pRingBuff->wp = buff;
	pRingBuff->rp = buff;
	pRingBuff->length = length;
	pRingBuff->flagOverflow = 0;
}


/**
  * @brief  清除ringbuffer结构体的相关信息
  * @param  pRingBuff：待处理的ringbuffer
  * @note   
  * @retval void
  * @author Acorss工作室	
  */	
  void rbClear(RingBuffer* pRingBuff)
{
 	pRingBuff->wp = pRingBuff->pBuff;
	pRingBuff->rp = pRingBuff->pBuff;
	pRingBuff->flagOverflow = 0;
}

/**
  * @brief  压入单字节到缓冲区
  * @param  pRingBuff：待处理的ringbuffer
  *         value：压入的数据
  * @note   
  * @retval void
  * @author Acorss工作室	
  */	
  void rbPush(RingBuffer* pRingBuff, uint8_t value)
{
	uint8_t* wp_next = pRingBuff->wp + 1;
	if( wp_next == pRingBuff->pEnd ) {
		wp_next -= pRingBuff->length; // Rewind pointer when exceeds bound
	}
	if( wp_next != pRingBuff->rp ) {
		*pRingBuff->wp = value;
		pRingBuff->wp = wp_next;
	} else {
		pRingBuff->flagOverflow = 1;
	}
}

/**
  * @brief  压出单字节到缓冲区
  * @param  pRingBuff：待处理的ringbuffer   
  * @note   
  * @retval 压出的数据
  * @author Acorss工作室	
  */	
  uint8_t rbPop(RingBuffer* pRingBuff)
{
	if( pRingBuff->rp == pRingBuff->wp ) 
		return 0; // empty
  
	uint8_t ret = *(pRingBuff->rp++);
	if( pRingBuff->rp == pRingBuff->pEnd ) {
		pRingBuff->rp -= pRingBuff->length; // Rewind pointer when exceeds bound
	}
	return ret;
}

/**
  * @brief  获取缓冲区尚未处理的字节数
  * @param  pRingBuff：待处理的ringbuffer   
  * @note   
  * @retval 待处理的字节数
  * @author Acorss工作室	
  */
  uint16_t rbGetCount(const RingBuffer* pRingBuff)
{
	return (pRingBuff->wp - pRingBuff->rp + pRingBuff->length) % pRingBuff->length;
}

/**
  * @brief  判断缓冲区是否为空
  * @param  pRingBuff：待处理的ringbuffer   
  * @note   
  * @retval 空为1；否则为0
  * @author Acorss工作室	
  */
  int8_t rbIsEmpty(const RingBuffer* pRingBuff)
{
	return pRingBuff->wp == pRingBuff->rp; 
}

/**
  * @brief  判断缓冲区是否空
  * @param  pRingBuff：待处理的ringbuffer   
  * @note   
  * @retval 满为1；否则为0
  * @author Acorss工作室	
  */
  int8_t rbIsFull(const RingBuffer* pRingBuff)
{
 	return (pRingBuff->rp - pRingBuff->wp + pRingBuff->length - 1) % pRingBuff->length == 0;
}

      /**
  ******************************************************************************
  * File Name          : dma.c
  * Description        : This file provides code for the configuration
  *                      of all the requested memory to memory DMA transfers.
  ******************************************************************************
  ** This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * COPYRIGHT(c) 2018 STMicroelectronics
  *
  * Redistribution and use in source and binary forms, with or without modification,
  * are permitted provided that the following conditions are met:
  *   1. Redistributions of source code must retain the above copyright notice,
  *      this list of conditions and the following disclaimer.
  *   2. Redistributions in binary form must reproduce the above copyright notice,
  *      this list of conditions and the following disclaimer in the documentation
  *      and/or other materials provided with the distribution.
  *   3. Neither the name of STMicroelectronics nor the names of its contributors
  *      may be used to endorse or promote products derived from this software
  *      without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "dma.h"

/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/*----------------------------------------------------------------------------*/
/* Configure DMA                                                              */
/*----------------------------------------------------------------------------*/

/* USER CODE BEGIN 1 */

/* USER CODE END 1 */

/** 
  * Enable DMA controller clock
  */
void MX_DMA_Init(void) 
{
  /* DMA controller clock enable */
  __HAL_RCC_DMA1_CLK_ENABLE();

  /* DMA interrupt init */
  /* DMA1_Stream3_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Stream3_IRQn, 0, 0);
  HAL_NVIC_EnableIRQ(DMA1_Stream3_IRQn);
  /* DMA1_Stream6_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 0, 0);
  HAL_NVIC_EnableIRQ(DMA1_Stream6_IRQn);

}

/* USER CODE BEGIN 2 */

/* USER CODE END 2 */

/**
  * @}
  */

/**
  * @}
  */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

      （ps：此处的dma可以提前在stm32cubemx中打开也可手动编写，个人推荐手动编写）

    2. 编写主函数
       

      int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
	HAL_Init();
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();
  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_DMA_Init();
  MX_NVIC_Init();
  MX_GPIO_Init();
  MX_USART1_UART_Init();
  Mavlink_Init();
  HAL_Delay(100);
  /* USER CODE BEGIN 2 */ 

  /* USER CODE END 2 */
	OLED_Init();
	OLED_Clear();
	OLED_ShowString(0,0,"Accel:",16,1);
	OLED_Refresh();	
	__HAL_RCC_GPIOC_CLK_ENABLE();
  /* USER CODE BEGIN WHILE */
while (1) {
		UserICM20948.Update_9axisVal(&imu_data);
		UserICM20948.UpdateAttitude(imu_data, &imu_attitude);
		mavlink_test();
		Loop_Mavlink_Parse();
          }
  /* USER CODE END 3 */
}

       

1.5 上机测试

  1. 测试可以一步步进行，先对串口进行简单的收发实验，保证串口能用，（ps：串口问题会遇到的问题主要是软件与硬件，硬件方面检查接线以及设备是否损坏，大概率硬件都会是好的，软件方面要注意系统内部时钟的配置，这一点很容易忽视导致无法发送数据，还有就是校验位频率等参数，上下位机一一对应就行。

  2. 等串口调好了就可以就可以开始着手mavlink方面，根据报错信息一步步解决，如果同时报很多错就说明缺少了一个重要文件（我会将文件开源，大家可以去下载）

1.6 代码

通过网盘分享的文件：uasr1.6 代码开源t_test1-1.zip

链接：https://pan.baidu.com/s/17L9NZg28UFXsoSh23iXMag?pwd=1234 https://pan.baidu.com/s/17L9NZg28UFXsoSh23iXMag?pwd=1234 

 

 二 、上位机（树莓派5）配置流程

2.1 环境配置

0.系统环境：ubantu24.04, 对应的ros2版本为jazzy

1.ROS配置

推荐先学习 B 站 up 主 小鱼 的 ROS2 入门教程：ROS2入门教程视频
安装ROS2可以用小鱼的ROS2一键安装命令：

wget http://fishros.com/install -O fishros && . fishros

随后跟着小鱼的课程, 只需安装一键安装包即可。

2.python虚拟环境

首先，我们要选定我们的工作区间。在这里我选择将工作区间放在桌面上。

sudo apt update
sudo apt install -y python3-venv
cd /home/a1/Desktop
python3 -m venv /home/a1/Desktop/ros2_venv

然后安装我们需要用到的库pymavlink

# 激活并升级 pip
source /home/a1/Desktop/ros2_venv/bin/activate
pip install --upgrade pip
# 安装pymavlink
pip install pymavlink

2.2 创建ros2功能包

cd /home/a1/Desktop/ROS2/imu_py/src
ros2 pkg create topic_py_1  --build-type ament_python --dependencies rclpy pymavlink

在topic_py_1/topic_py_1下创建topic_publisher.py接着我们开始编写代码

import rclpy
from rclpy.node import Node
from pymavlink import mavutil
from example_ros2_interfaces.msg import Mymsg
import time


class NodePublisher(Node):
    def __init__(self,name, com='/dev/ttyACM0', baud=115200):
        super().__init__(name)
        self.get_logger().info("大家好，我是%s!" % name)
        self.command_publisher_ = self.create_publisher(Mymsg,"/imu", 10) 

        self.connection = mavutil.mavlink_connection(com, baud)

        self.timer = self.create_timer(0.1, self.timer_callback)

        self.time_count = 0
        self.timezz = 0.0

        # # 新增：缓存上一帧有效 IMU 数据（初始化为重力 Z）
        self._LAST_DATA = (0.0, 0.0, 0.0, 9.8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
    
    def get_imu_data1(self, timeout=0.05, flush_limit=20):
        """
        在 timeout 时间窗口内非阻塞读取，获取最新一帧 HIGHRES_IMU。
        没有新数据则返回上一帧缓存，绝不返回 None。
        """    
        if self.time_count == 0:
            self.timezz = time.time()
            self.time_count=1

        end_t = time.time() + timeout
        flush_count = 0
        while time.time() < end_t and flush_count < flush_limit:
            msg = self.connection.recv_match(blocking=False)
            if msg is None:
                time.sleep(0.002)
                continue
            flush_count += 1
            if msg.get_type() == 'HIGHRES_IMU':
                ts = float(float(msg.time_usec) / 1e6 + self.timezz)
                self._LAST_DATA = (
                    ts,
                    float(msg.xacc), float(msg.yacc), float(msg.zacc),
                    float(msg.xgyro), float(msg.ygyro), float(msg.zgyro),
                    float(msg.xmag), float(msg.ymag), float(msg.zmag)
                )
        return self._LAST_DATA

    def timer_callback(self):
        """
        定时器回调函数
        """
        msg = Mymsg()
        msg.timestamp ,msg.xacc, msg.yacc, msg.zacc,msg.xgyro,msg.ygyro, msg.zgyro, msg.xmag, msg.ymag, msg.zmag = self.get_imu_data1()
        self.command_publisher_.publish(msg)
        self.get_logger().info(
            f'发布：{msg.timestamp:.2f},{msg.xacc:.2f},{msg.yacc:.2f},{msg.zacc:.2f},'
            f'{msg.xgyro:.2f},{msg.ygyro:.2f},{msg.zgyro:.2f},'
            f'{msg.xmag:.2f},{msg.ymag:.2f},{msg.zmag:.2f}'
        )

def main(args=None):
    rclpy.init(args=args)
    node = NodePublisher("maclink_IMU")
    rclpy.spin(node)
    rclpy.shutdown()

将上述代码拷贝进topic_publisher.py

这里用的了一个自定义接口(自己跟着教程学或者下载百度网盘已经打包好的功能包)，放到/home/a1/Desktop/ROS2/imu_py/src下

【ROS2机器人入门到实战】ROS2接口介绍_话题接口-CSDN博客

通过网盘分享的文件：example_ros2_interfaces
链接: https://pan.baidu.com/s/1PXJ5gQhRa0dr2Jt6W8mK0g 提取码: knd8

接口协议：

float64 timestamp
float64 xacc
float64 yacc
float64 zacc
float64 xgyro 
float64 ygyro 
float64 zgyro 
float64 xmag 
float64 ymag 
float64 zmag
float64 relative_ts

然后修改/home/a1/Desktop/ROS2/imu_py/src/topic_py_1/setup.py

from setuptools import find_packages, setup

package_name = 'topic_py_1'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools',
                        'rclpy',
                        'pymavlink',],
    zip_safe=True,
    maintainer='a1',
    maintainer_email='a1@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            "pub1=topic_py_1.topic_publisher:main", 
        ],
    },
)

最后编译

cd /home/a1/Desktop/ROS2/imu_py
colcon build

这样我们就可以得到一个成功通过mavlink传输的陀螺仪数据，并且以/imu话题发布。

2.3 启动脚本

由于我们的ros2功能包中使用了第三方环境中的库，所以需要特殊的方式启动才能使得运行时程序成功导入库

创建脚本/home/a1/Desktop/run_with_env.sh

#!/bin/bash
 
# 设置ROS2环境
source /opt/ros/jazzy/setup.bash
source /home/a1/Desktop/ROS2/imu_py/install/setup.bash

# 设置PYTHONPATH包含指定的python包路径
export PYTHONPATH="/home/a1/Desktop/ros2_venv/lib/python3.10/site-packages:$PYTHONPATH"

# 显示当前PYTHONPATH（调试用）
echo "当前PYTHONPATH:"
echo $PYTHONPATH | tr ':' '\n'
echo "------------------------"

# 检查pymavlink是否可用
python3 -c "
try:
    from pymavlink import mavutil
    print('✓ pymavlink 模块可用')
except ImportError as e:
    print('✗ pymavlink 模块不可用:', e)
"

# 运行节点
ros2 run topic_py_1 pub1

然后直接在终端启动该脚本即可

/home/a1/Desktop/run_with_env.sh

以下是输出示例：

数据从左到右依次是:

msg.timestamp 实时时间

msg.xacc 加速度计 X

msg.yacc加速度计 Y

msg.zacc加速度计 Z

msg.xgyro陀螺仪 X

msg.ygyro陀螺仪 Y

msg.zgyro陀螺仪 Z

msg.xmag磁力计 X

msg.ymag磁力计 Y

msg.zmag磁力计 Z

2.4 测试：创建一个订阅者读取imu数据

在topic_py_1/topic_py_1下创建topic_subscribe.py接着我们开始编写代码

import rclpy
from rclpy.node import Node
from example_ros2_interfaces.msg import Mymsg

class NodeSubscribe02(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("大家好，我是%s!" % name)
        # 创建订阅者
        self.command_subscribe_ = self.create_subscription(Mymsg,"/imu",self.command_callback,10)

    def command_callback(self,msg):
        self.get_logger().info(f'收到了指令：{msg.timestamp:.2f},{msg.xacc:.2f},{msg.yacc:.2f},{msg.zacc:.2f},{msg.xgyro:.2f},{msg.ygyro:.2f},{msg.zgyro:.2f},{msg.xmag:.2f},{msg.ymag:.2f},{msg.zmag:.2f}')    #打印一下发布的数据
        

def main(args=None):
    rclpy.init(args=args)
    node=NodeSubscribe02("sub1")
    rclpy.spin(node)
    rclpy.shutdown()

接着修改/home/a1/Desktop/ROS2/imu_py/src/topic_py_1/setup.py

from setuptools import find_packages, setup

package_name = 'topic_py_1'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools',
                        'rclpy',
                        'pymavlink',],
    zip_safe=True,
    maintainer='a1',
    maintainer_email='a1@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            "pub1=topic_py_1.topic_publisher:main",
            "sub1=topic_py_1.topic_subscribe:main"
        ],
    },
)

然后先启动发布者,在启动订阅者

cd /home/a1/Desktop/ROS2/imu_py
colcon build
# 开启发布者
/home/a1/Desktop/run_with_env.sh

# 设置ROS2环境
source /home/a1/Desktop/ROS2/imu_py/install/setup.bash
 
# 开启订阅者
ros2 run topic_py_1 sub1

输出示例：

2.5 代码

通过网盘分享的文件：example_ros2_interfaces
https://pan.baidu.com/share/init?surl=PXJ5gQhRa0dr2Jt6W8mK0g https://pan.baidu.com/share/init?surl=PXJ5gQhRa0dr2Jt6W8mK0g  提取码: knd8

 

# stm32 # 嵌入式硬件 # 单片机 # 学习 # ubuntu
Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

cover

第232篇 运动学约束规划——速度/加速度/加加速度限制的处理

avatar DAMO开发者矩阵
cover

第229篇 轨迹时间参数化——TOPP-RA和速度约束处理

avatar DAMO开发者矩阵

景区导览机器人项目避坑指南：从技术选型到落地交付的完整复盘

2026 年，随着新版国家标准《旅游景区质量等级划分》（GB/T 17775—2024）全面落地，“智慧旅游” 首次独立成项，分值 110 分。与此同时，《旅游景区讲解服务规则》（LB/T 014‑2026）首次对 “智慧讲解” 服务提出系统性要求。智慧旅游成为独立评分大项，智能导览机器人是拿分的重要硬件方案之一。景区导览机器人项目，看起来是“买几台设备”，实际涉及AI交互、多语种翻译、路径规划、

avatar DAMO开发者矩阵