STM32下mavlink的使用个人总结（包含对ACfly里面mavlink的分析，包含接收T265的位置信息的二次开发教程）

阅读量：4083 次

发布时间：2019-05-25

本文共 30405 字，大约阅读时间需要 101 分钟。

我一开始想既然mavlink在STM32的使用只需要调用函数就可以了，但是mavlink在STM32的代码似乎比较多，我就想起直接看它的头文件有哪些函数，结果查看头文件有巨大发现

关于msg id的确定看我这两篇博文：

对应每种消息都有专门一个头文件，我甚至找到了vision_positon_estimate的头文件，是不是要读取某类消息只需要调用这个对应消息头文件里面的函数就可以了。

是不是在驱动文件include这个头文件就可以了，然后直接调用就可以了。好像main文件要Include mavros.h

在这个头文件里面我找到了似乎是对应读取某个参数的，这么来看用mavlink其实挺方便的。

那我再写驱动或者ACfly的传感器二次开发很方便了，都不需要自己写协议解析，就调用一个函数得到数据之后直接传给更新函数就行了。

static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavlink_message_t* msg){    return _MAV_RETURN_uint64_t(msg,  0);}/** * @brief Get field x from vision_position_estimate message * * @return [m] Global X position */static inline float mavlink_msg_vision_position_estimate_get_x(const mavlink_message_t* msg){    return _MAV_RETURN_float(msg,  8);}/** * @brief Get field y from vision_position_estimate message * * @return [m] Global Y position */static inline float mavlink_msg_vision_position_estimate_get_y(const mavlink_message_t* msg){    return _MAV_RETURN_float(msg,  12);}/** * @brief Get field z from vision_position_estimate message * * @return [m] Global Z position */static inline float mavlink_msg_vision_position_estimate_get_z(const mavlink_message_t* msg){    return _MAV_RETURN_float(msg,  16);}/** * @brief Get field roll from vision_position_estimate message * * @return [rad] Roll angle */static inline float mavlink_msg_vision_position_estimate_get_roll(const mavlink_message_t* msg){    return _MAV_RETURN_float(msg,  20);}/** * @brief Get field pitch from vision_position_estimate message * * @return [rad] Pitch angle */static inline float mavlink_msg_vision_position_estimate_get_pitch(const mavlink_message_t* msg){    return _MAV_RETURN_float(msg,  24);}/** * @brief Get field yaw from vision_position_estimate message * * @return [rad] Yaw angle */static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_message_t* msg){    return _MAV_RETURN_float(msg,  28);}/** * @brief Get field covariance from vision_position_estimate message * * @return  Pose covariance matrix upper right triangular (first six entries are the first ROW, next five entries are the second ROW, etc.) */static inline uint16_t mavlink_msg_vision_position_estimate_get_covariance(const mavlink_message_t* msg, float *covariance){    return _MAV_RETURN_float_array(msg, covariance, 21,  32);}/** * @brief Decode a vision_position_estimate message into a struct * * @param msg The message to decode * @param vision_position_estimate C-struct to decode the message contents into */static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_vision_position_estimate_t* vision_position_estimate){#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS    vision_position_estimate->usec = mavlink_msg_vision_position_estimate_get_usec(msg);    vision_position_estimate->x = mavlink_msg_vision_position_estimate_get_x(msg);    vision_position_estimate->y = mavlink_msg_vision_position_estimate_get_y(msg);    vision_position_estimate->z = mavlink_msg_vision_position_estimate_get_z(msg);    vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg);    vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg);    vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg);    mavlink_msg_vision_position_estimate_get_covariance(msg, vision_position_estimate->covariance);#else        uint8_t len = msg->len < MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN? msg->len : MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN;        memset(vision_position_estimate, 0, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN);    memcpy(vision_position_estimate, _MAV_PAYLOAD(msg), len);#endif}

还有一个问题，怎么给mavlink指定串口。指定是用哪个串口接收和发送。

下面这里有讲到，这篇文章我也应该转载过。

这篇也讲到了

这下面的pressure应该泛指某一类消息，比如可以用vision_positon_estimate替代掉pressure

而且下面说了接收消息就是我上面找到的那几个get函数！！！！！！

这个mavlink_parse_char函数我也找到了，是就在 mavlink_helpers.h中，直接ctrl + F可以搜到。

那它是在哪里指定用哪个串口的呢。

这个函数前面的注释里面有对各个输入参数解释，似乎第一个参数就和串口有关？

chan应该就是channel的简写，应该就是通道的意思！！！！！！

/** * This is a convenience function which handles the complete MAVLink parsing. * the function will parse one byte at a time and return the complete packet once * it could be successfully decoded. This function will return 0 or 1. * * Messages are parsed into an internal buffer (one for each channel). When a complete * message is received it is copies into *returnMsg and the channel's status is * copied into *returnStats. * * @param chan     ID of the current channel. This allows to parse different channels with this function. *                 a channel is not a physical message channel like a serial port, but a logic partition of *                 the communication streams in this case. COMM_NB is the limit for the number of channels *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows * @param c        The char to parse * * @param returnMsg NULL if no message could be decoded, the message data else * @param returnStats if a message was decoded, this is filled with the channel's stats * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC * * A typical use scenario of this function call is: * * @code * #include 
   
     * * mavlink_message_t msg; * int chan = 0; * * * while(serial.bytesAvailable > 0) * { *   uint8_t byte = serial.getNextByte(); *   if (mavlink_parse_char(chan, byte, &msg)) *     { *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); *     } * } * * * @endcode */MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status){    uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);    if (msg_received == MAVLINK_FRAMING_BAD_CRC ||	msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) {	    // we got a bad CRC. Treat as a parse failure	    mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);	    mavlink_status_t* status = mavlink_get_channel_status(chan);	    _mav_parse_error(status);	    status->msg_received = MAVLINK_FRAMING_INCOMPLETE;	    status->parse_state = MAVLINK_PARSE_STATE_IDLE;	    if (c == MAVLINK_STX)	    {		    status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;		    rxmsg->len = 0;		    mavlink_start_checksum(rxmsg);	    }	    return 0;    }    return msg_received;}

目前我的理解是，main函数里面include mavlink.h就可以，然后直接调用相应的接收函数，现在问题是那个端口怎么定义的。

ACfly里面mavlink端口方面的

关于端口ACfly就是在commulink.cpp和commulink.h里面弄的！！！！像这个头文件里明确有写注册端口

它应该mavlink里面有某个函数调用了串口接收或者发送函数，我觉得你去深挖那个mavlink自己的接收函数应该是可以最终找到那个串口的函数的。

这是那个恒力久行自己写的一个头文件，他的教程里是要把他写的这个头文件加进去。

是的，一层层从

mavlink_parse_char函数开始深挖下去，可以找到一些线索。

ACfly里面mavlink通信似乎也是一个单独的任务，就在这个文件里面，我似乎要加mavlink通信只需要在这加就可以了，不需要去main函数加，不需要去主文件那include mavlink.h 而是在这个文件里面调用API函数。端口什么的他应该定义好了我可以在这里面直接用，不需要折腾了。

是不是也就意味着其他任务是没法直接获得mavlink消息的，通过任务间通信来弄，你也不需要在驱动里面去写mavlink，

目前可以知道，每一个传感器都会创建一个线程，我看了下单单uart驱动是没有创建单独线程的，uart驱动应该是在传感器线程里面被调用的。

我觉得mavlink就可以理解为一个传感器驱动，这个驱动它底层应该是调用串口驱动，mavlink本身应该只需要做些协议解析即可，应该不复杂，为什么看到写这么多呢。应该几个函数就可以搞定。

本身这个commulink.cpp（或者说这个任务）里面也有任务间通信的函数

我在mavlink_helpers.h搜索uart有了新发下，就下面标为蓝色的这段代码，

if (chan == MAVLINK_COMM_0)

{

uart0_transmit(ch);

}

if (chan == MAVLINK_COMM_1)

{

uart1_transmit(ch);

}

看到了吧，如果chan == MAVLINK_COMM_0 就调用 uart0_transmit(ch);这里真正把chan和具体的串口驱动函数关联了起来！！！！！也是我之前想的它肯定会调用串口驱动函数的。可是我刚刚发现 uart0_transmit（）这个函数在其他地方搜不到，也就是可能串口驱动里面没这个函数。。

然后其他函数再调用这个函数 _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) 这个函数就是调用comm_send_ch(mavlink_channel_t chan, uint8_t ch) 实现的！！！！其他函数又大量调用_mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) 这个函数，这个可以通过搜索发现！！！！

我估计接收应该也有一个相应的函数

我这还发现一个mavlink_get_info.h的头文件。

有个新发现

这个也是0

这个也是0就在我这篇文章下面一些

我现在在整个ACfly工程搜索mavlink_parse_char，只有一处地方引用，就是在commulink.cpp里面，这里面你可以清楚看到i怎么来的。

i就是这么来的，而且可以看到513行注释了i就是chan

而且又看到这个关键词 MAVLINK_COMM_NUM_BUFFERS

找到它的定义

可以直接百度这个函数，也可以找到不少。

commulink.cpp里面专门有一串代码是来寻找可用的端口的。这样逻辑就清晰许多！！！！！！

讲道理ACfly应该是用uart1这个串口进行mavlink的通信，所以它这个应该是定了的。

我们再回看一下chan官方在头文件里面的解释说不是一个物理通道，

又有个惊天的发现，UART1驱动文件里面的端口注册函数就是调用的commulink.cpp里面的，莫非每个UART端口在使用之前都要注册，分配一个逻辑号码么。

现在又有个更大的发现，UART3的驱动文件特地把端口注册注释掉了，是不是因为UART1专门用来mvalink通信的所以需要端口注册！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！这样就解释通了！！！！！！！！！！！UART5里面也没有端口注册。

你再细看那个端口注册函数，得到号码之后紧接着就是mavlink的一些操作！！！！！！

所以实际上是不用选哪个端口的，ACfly就是用UART1进行mavlink的通信。

上面标蓝的第一个函数 mavlink_init_chan 我估计就是初始化端口的，看了下这个函数的定义，就是mavlink.cpp里面第一个个函数！！！！！！！！mavlink.cpp之前我从mavlink_parse_char函数开始深挖下去，也最后是挖到这个文件，上面有记录。

目前我感觉至少现在叫我写个mavlink接收，我用mavlink_parse_char这个函数，第一个参数chan我应该知道怎么写了！！！！！，目前我应该可以完成一个mavlink消息的接收了！！！！！！！！！

然后我看了下mavlink_parse_char这个函数在整个ACfly里面唯一被调用的地方就是commulink.cpp的while(1)里面，那么那个chan是什么时候确定的呢，我从while(1)的开头开始找，发现了，它确定i的值也就是chan的值得方法非常简单粗暴，就是看哪个端口有值写入，就我下面标位蓝色的那行，也就是后面的i值都由这里确定！！！！！！！！！还有这个while（1）实际是mavlink这个任务的执行函数Commulink_Server里的，你学了freertos也知道一个任务就是一个while(1)，现在你应该有更为清醒的认识。

还有我发现Commulink.cpp上面的注册端口部分的函数似乎并没有在while(1)里面调用，似乎只是UART1的驱动文件里面UART1的初始化函数调用了它，莫非i是先UART1初始化的时候确定，后面要mavlink接收的时候，就逐个判断哪个i是有数据写入的就确定是哪个i么？为什么感觉这样有点麻烦。

这是后面调用mavlink_parse_char函数时，chan参数的位置就写着i

comport[]是port数据类型，在commulink.h里有定义

我主要是想知道它这端口到底指的什么端口，怎么判断端口有没有数据写入。

但是想查看这个函数的定义跳转不了，如果能看到这个函数的定义就知道它这里的端口指什么端口了，这种相当于什么，函数指针？

我找到了！！！！！！！！！！！！write=Write_Uart1 !!!!!!!!

也就是判断端口有没有输入就是判断UART1有没有输入！！！！！！！

因为我是一开始注意到CommuPortRegister函数的输入参数是UartPort，再仔细找UartPort就是port类型！！！！！然后看到write！！！！！！！正好就等于Write_Uart1，所以实际上不管i是几，都是调用的uart1的函数，都是在用uart1口。因为port类型里面的wirte都是调用的UART1的wirte，comport[]是port数据类型，comport[i]不管里面i是几，也就是不管chan是几，都是用的UART1口。那分成几个i是为了什么，为了并行发送数据么？因为操作系统？可以看着像并行的？

我又想起那个官方的定义

什么叫通信流的逻辑分区？莫非是一个串口可以有多个通信流，这样子么。一个串口支持多个通信流，是不是要看串口寄存器什么的？似乎不是，官方说的是单片机的通信流的数量。

COMM_NB

还是说这是使用串口的标准用法，每次用一个串口前，都要这样先获取一个i么

这里有对MAVLINK_COMM_NUM_BUFFERS 官方的解释，难道是i就是代表一个串口有几个缓冲区么，i的最大值就是MAVLINK_COMM_NUM_BUFFERS 这是串口本身就有这种缓冲区还是mavlink这样的？为什么每次用之前要先确定i呢，是一次只能用一个缓冲区么，可能是一个串口有四个缓冲区，还有那个wirte是串口发送函数，发送之前先确定哪个缓冲区为空？

ACfly里面那个缓冲区要么为4 要么为16是不是对应4个字节或者16个字节，也就是32b或者128b？

ACfly的用户手册里面也有说环形缓冲区！！！！，我搜UART缓冲区大小，出来的结果基本是环形缓冲区！！！！！

可能真的是缓冲区的意思，所以分为单片机和linux

我直接百度搜mavlink 缓冲区又有重大发现，别人好像是有专门写Mavlink缓冲区这一块，在移植到STM32的时候

这里特地说了缓冲区对于接收来说很有必要！恒久力行专门花一篇文章来写这个，他说之前没有加缓冲区，没有接收函数功能！！！！说明那个接收函数mavlink_parse_char的chan应该是指缓冲区！！！本身接收的话第一个函数就是它啊，似乎只接收需要缓冲区，发送不需要，所以我之前弄的一个mavlink的STM32例程里面没有缓冲区这些似乎，可能因为他只发送，没有接收消息。（不过我又好像看到发送也是需要buf的，至少函数名称是这样）

这个似乎说的就是 mavlink_parse_char 这个函数是从缓冲区中读取的？也就是那个chan就是指的缓冲区。

mavlink_channel_t 找下这个定义，似乎是一个结构体？

我在这里也看到了stream,可能和chan的解释中的stream是一个意思。是不是再次说明chan是缓冲区？

我看mavlink的官方手册，有了惊天大发现！！！！！可以看我这篇博文：

我发现接收函数mavlink手册就有讲，

mavlink_parse_char

你是一个劲百度就不肯看mavlink手册

更有讲到这个非常关键的东西，现在应该也明白了这个MAVLINK_COMM_NUM_BUFFERS 为什么为4或者16，应该就是来自于这里

感觉和这里的说法差不多，这是mavlink代码里面自带的注释

这里还可以看出通道和通信缓冲区是一个概念，

怪不得有的教程说直接写0 ！！！！！！

到底怎么理解多个通道呢，一个程序里面有多个mavlink流.....mavlink流指什么呢。一种mavlink消息就是一个流么。

弄多个通道的意义何在？实际还不是串行发送接收？你要接收一个数据那就直接调用串口接收嘛。莫非是串口可能传来多种mavlink消息？而你又不能总是让一种消息的的接收函数在那霸占着串口？是的，你现在想一个问题mavlink传来多种消息，你怎么区分？这个问题怎么解决？怎么解决？

我现在再仔细分析一下那个接收函数，发现所谓的缓冲区本质是一个装着 mavlink_message_t 类型的数组

首先看

然后我们再回看这个接收函数，这个函数里面有个关键的函数，从名字就可以看出来，mavlink_get_channel_buffer(chan);就是从通道、缓存区获得对应的缓存（buffer）的，也就是那个通道里的值，我们细挖这个函数。

会发现这个函数返回的是一个 m_mavlink_buffer[chan] ，说明通道、缓存区存放的是这个，那我们深挖这个，

深挖会发现这个是一个 mavlink_message_t 类型的数组，和上面解释的没错，接收函数返回的也是这么一个数据类型，那我们去看看 mavlink_message_t 这个数据类型是什么

就回到了我们上面的地方，这应该就是一个mavlink消息的结构体。也就是所谓的缓冲区的本质就是存放着一个一个的mavlink消息！！！！！就是一个mavlink消息数组，所谓的通道、缓冲区本质是这个。MAVLINK_COMM_NUM_BUFFERS 为4就是说这个数组长度为4，我觉得它按照通道来读取的意义就是我可以比如一个数组 a[0] a[1] a[2] a[3] ，如果不用通道统一接口读取，可能我得先读了a[1]，才能读a[2],读了a[2]才能读a[3]，也不一定，其实我们就可以默认chan为0，就每次读取0位置的就可以了，应该也不影响什么。不对，还是有点乱，但是还是弄清了一些本质。而且这个数组是认为定义的，不是说硬件缓冲区就这么大，不是的，就人为定义一个数据，串口接收到一个mavlink消息就一次放到这个数组里面，我感觉读取按照FIFO就依次读取就好了。莫非它读取还要先判断一下数组里面哪个位置有值再去读取？那这样就不是栈了，可能是其他数据结构，不对，数据确实可以直接按照下标读取，除非是这样，某一种消息专门放到数组的一个位置，这么操作，是不是好些。也就是它的缓冲区一次可以存放四种mavlink消息，你要取对应的mavlink消息就去数组对应的位置去取就好了。但是也不对，串口是分不清你是哪种mavlink消息的，串口是没有对mavlink消息包做解析的，所以这么想也不合理，我感觉这么设计应该是给多种mavlink消息设计的，如果只有一种mavlink消息没必要如此。

我发现他这建立了MAVLINK_COMM_NUM_BUFFERS这么多个互斥信号量，应该就是给那个缓冲区的，可能就是读取缓冲区的时候。

看mavlink.cpp可以更清楚这一点，对chan的初始化似乎就是分配信号量，解锁chan就是释放信号量。

我想找找UART1串口是怎么把数据放到缓冲区里面的，这样就真相大白了。我们现在知道要去那里读，但是是谁把mavlink消息放到那里面的呢？它要放到缓冲区数组的哪个位置里面似乎只需要commuport[i].read就可以了。这又不太理解了。可能这个i就是一个逻辑上的人为定义。因为不管你i等于几，commuport[i].read都是调用的完全一模一样的函数！！！！！，那么意味着消息也是一样进行存放的。并不是你i为几他就把接收到的数据放到几，没有，UART1的读取函数就一个。那是如何和那个缓冲数组联系起来的？

不过这程序好理解了，我放到哪个i，下面mavlink_parse_char就去哪个i读取！！！！！这个OK。

深挖会发现它们串口用到了Freertos的流和缓冲，这可能比单纯的串口又要复杂些了。。。。

你要不先把那个UART的串口程序真正啃懂，我觉得那个啃懂啃明白了很多东西应该自然明白了，估计那综合了串口和freertos，应该也可以让你得到很好的锻炼。

真正搞清楚串口收到消息后放在哪里，和i有没有关系
port[i].read，i不同，讲道理调用的串口读取函数是一样的，那数据的存放和i的关系是什么，把这个弄清楚是关键，是否是通过操作系统的互斥信号量来实现，存放的位置不同?

这是commulink.cpp接收mavlink消息的代码，应该也是整个ACfly里面唯一一个接收mavlink消息的代码，我来做个完整的注释

if( CommuPorts[i].read != 0 ) //maxi:read对应着UART的串口接收函数，如果返回的不是0代表接收到了消息。既然不同I调用的都是同一个串口接收函数返回值不应该是一样的么，那这种判断有什么意义。			{	//接收数据处理				//每次接收64个字节进行处理				mavlink_message_t msg;				uint8_t buf[64];				uint8_t length;							do				{					length = CommuPorts[i].read( buf, 64, 0, 0.01 );   //maxi:这实际是调用的UART的接收函数。这里实际调用read的时候还是输入了参数的！！！！！！上面判断的时候没有，这个Buf很关键应该，是个地址，数组首地址。//maxi：应该是将接收的数据压入缓冲区。					for( uint8_t k = 0; k < length; ++k )					{						//消息解包						if( mavlink_parse_char( i, buf[k], &msg, NULL ) == MAVLINK_FRAMING_OK )  //maxi:这个函数应该再熟悉不过了。						{							//消息解包完成														//如果消息处理函数存在							//处理消息							if( msg.msgid < Mavlink_RC_Process_Count )							{								if( Mavlink_RC_Process[ msg.msgid ] != 0 )									Mavlink_RC_Process[ msg.msgid ]( i , &msg );							}						}					}				}while( length > 10 );

我分析一下那个串口接收函数，看它到底把接收到的数据放到哪里了，最关键的就是那个标蓝的函数，我深挖一下，

我感觉目前还是先把那两个函数真正看懂，一个串口接收函数，一个那个mavlink读取函数，真正把函数体内的东西读懂。我觉得很多东西就可以弄明白了

这段关键代码也就是包含上面两个函数。真正把这两个函数彻底弄懂我觉得就差不多了。

串口接收函数放到一个地方，mavlink接收函数讲道理就要去解析那个东西，那这个i到底起什么作用。

我好像理清了！！！

这里可以看到那逻辑端口开始定义的时候都是赋值为0！！！！！！！！！！

这里找可用的逻辑端口，CommuPorts[ i ].read == 0 read是一个指针，也就是一个地址，判断一个端口是否可用，如果一个逻辑端口绑定了一个实际端口，这个指针就会指向实际的UART_read函数，就不会为0了，这里实际是判断一个逻辑端口有没有绑定物理端口，如果没有，就去把它绑定物理端口。并不是我以前理解的有没有输入输出，这还是怪我对那个函数指针没有弄到位，看port的结构体定义没有意识到里面是几个指针，而不是函数！！！！！！所以可以这样写CommuPorts[ i ].read == 0 而没有输入参数。

现在你再看这个不用纠结了，i我感觉都可以忽略掉，那就是在用实际的UART1的read函数。

我现在至少弄清楚了chan和物理端口的关系，这么来看 i似乎一般就为0了。

刚接收到的数据你还没有解包的时候你分不清楚哪些是mavlink消息包，怎么可能放到那个缓冲区也就是mavlink消息数组里面呢，是不是解包完成后才放到那里面？

应该是解析出来的消息包放在那里面才差不多。不然是分不清楚的，解析出来的消息包，放在那里，再由其他任务读取或者什么的都可以。

似乎处理后的消息是放到msg里面了。msg也是一个 mavlink_message_t 类型。

接收函数的注释里面也说了，就下面标蓝的部分，解析到一个内部buffer，当一个完整的消息接收到了，它被复制到*returnMsg

是的，这注释可以仔细读读。

这个函数似乎是接收函数层层深挖的最关键的一个实现函数，真正的实现似乎就在这里面，可以好好看它的注释，

可以这么理解，串口那边是得到一个字节一个字节的数据，我们这边呢，先放64个字节到buf里面，然后一个字节一个字节解析，解析完完整的数据再返回完整的数据。

我现在可以完全确定，那个通道就是用来放解析出来的每个字节的数据，直到解析完整个消息，再返回整个消息，起的作用就是这样！！！！！！！！！！多个通道，本质应该是多个缓冲区，这样可以去同时解析多个数据，而不用等着一个缓冲区用完，再解析下一个。

看官方的解释太重要了！！！！哪怕是英文也仔细看，单词一个意思一个意思地查，不要整段复制过去翻译，就要自己去亲自看论文。你不去查parse的意思，你就不能真正读懂。

这里明确地说了，消息就是被协议到一个内部的buffer，这个buffer是每个通道一个！！！！！！这不就是那个缓冲数组么！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！所以你现在也不用纠结啊怎么之前没找到串口接收把数据放到缓冲数组里。如我前面所说，串口接收到的一个字节一个字节的数据，怎么分得清是什么消息，而且把一个完整的消息放到一个消息类型的数组里，不可能的，这是解析出来之后放的，而且应该是放满一个消息就返回一个整个消息。因为一个消息很多个字节，要一个字节一个字节解析，我们深入到这种本质才好理解。

现在我估计都有底气去看芯片的英文手册了，以后看到英文文档不要怕。自己亲自一句一句去看，遇到不懂的单词就查，别拿去整段翻译。

之前明白chan通道就是缓冲区，也是看的官方的解释的。

现在也让我对串口通信等等有了更深的认识。经过真正mavlink通信的开发。你必须深入底层理解其本质。

得到对应的数据之后传感器的更新函数是不是可以直接写在这里面了。注册函数也写在这里面？是的，就写在mavlink线程里面，这样线程创建也不需要自己弄了。本身ACfly的传感器驱动程序就是做协议解析，这里不是把MVALINK协议解析了么，不就可以了，我觉得而应该是可以的。

就不需要再写什么任务间的消息通信了。

一个重大发现，drv_main.cpp里面已经把init_commulink（）写进去了！！！！！，确实就是当做一个传感器，我只需要在init_commulink（）加入注册函数，更新函数就OK了。！！！！！！！！！

之前我整理的ACfly传感器二次开发，需要注意的是四点，第一个是传感器的驱动文件（主要是协议解析，当然还要创建这个传感器任务），第二个传感器注册函数，第三个传感器更新函数，这两个都是在传感去驱动文件里面调用，第四个，把这个传感器的Init函数加到drv_main.cpp文件里面，上面除了注册函数更新函数其他的可以直接借助Mavlink现有的程序就可以了。

下面的截图里包含了我添加的部分，已经加了更新函数。

这个传感器的排序，我之前没有细思考什么排在前面什么排在后面。就是优先级光流的优先级反而排在最后面

超声波的优先级是1

TFmini的优先级是2

气压计的优先级是3

GPS的优先级是6

光流的优先级是8

那我先把T265的优先级定为9吧，让光流先。。。最开始安全起见，后面可以慢慢升高优先级。

后来和ACfly聊了下他们最近的代码逻辑变了，这不是优先级，改为信任度了。可以去看聊天记录。

我自己已经写好而且编译0错误

是的，只需要改动commulink.cpp就可以了。我把commulink.cpp放在这下面。

#include "Commulink.hpp"#include "Basic.hpp"#include "FreeRTOS.h"#include "task.h"#include "drv_LED.hpp"#include "mavlink.h"#include "MavlinksendFuncs.hpp"#include "MavlinkRCProcess.hpp"#include "MeasurementSystem.hpp"#include "Sensors.hpp"#include "Parameters.hpp"#include #include "SensorsBackend.hpp"//maxi:最后一个头文件SensorsBackend.hpp是我自己加的using namespace std;/*声光提示*/	static float ledSignalCounter = -1;	static LEDSignal ledSignal;	static LEDMode ledmode = LEDMode_Processing1;	static float ledR = 0, ledG = 0, ledB = 0;	static bool buzzerOn = false;	static uint16_t buzzerFreq;	void sendLedSignal( LEDSignal signal )	{		ledSignalCounter = 0;		ledSignal = signal;	}	void setLedMode( LEDMode mode )	{		ledmode = mode;	}	void setLedManualCtrl( float R, float G, float B, bool BuzzerOn, uint16_t BuzzerFreq )	{		ledR = R;	ledG = G;	ledB = B;		buzzerOn = BuzzerOn;	buzzerFreq = BuzzerFreq;		ledmode = LEDMode_Manual;	}	static inline void LEDRefresh(float dt)	{		if( ledSignalCounter >= 0 )		{			switch(ledSignal)			{				case LEDSignal_Start1:				{					if( ledSignalCounter > 0.3 )					{						ledSignalCounter = -1;						return;					}					if( ledSignalCounter < 0.15f )					{						set_BuzzerFreq(900);						set_BuzzerOnOff(true);						set_LedBrightness( 0, 0, 100 );					}					else					{						set_BuzzerFreq(1500);						set_BuzzerOnOff(true);						set_LedBrightness( 0, 100, 0 );					}					break;				}				case LEDSignal_Start2:				{					if( ledSignalCounter > 0.45 )					{						ledSignalCounter = -1;						return;					}					if( ledSignalCounter < 0.15 )					{						set_BuzzerFreq(800);						set_BuzzerOnOff(true);						set_LedBrightness( 100, 0, 0 );					}					else if( ledSignalCounter < 0.3 )					{						set_BuzzerFreq(1000);						set_BuzzerOnOff(true);						set_LedBrightness( 0, 100, 0 );					}					else					{						set_BuzzerFreq(1200);						set_BuzzerOnOff(true);						set_LedBrightness( 0, 0, 100 );					}					break;				}								case LEDSignal_Continue1:				{					if( ledSignalCounter > 0.8 )					{						ledSignalCounter = -1;						return;					}					set_BuzzerFreq(1500);									if( ledSignalCounter < 0.2f )					{						set_LedBrightness( 0, 0, 100 );						set_BuzzerOnOff(true);					}					else if( ledSignalCounter < 0.4f )					{						set_LedBrightness( 0, 0, 0 );						set_BuzzerOnOff(false);					}					else if( ledSignalCounter < 0.6f )						set_LedBrightness( 0, 0, 100 );					else						set_LedBrightness( 0, 0, 0 );					break;				}								case LEDSignal_Success1:				{					if( ledSignalCounter > 0.8 )					{						ledSignalCounter = -1;						return;					}					set_BuzzerFreq(1500);					if( ledSignalCounter < 0.2f )					{												set_LedBrightness( 0, 100, 0 );						set_BuzzerOnOff(true);					}					else if( ledSignalCounter < 0.4f )					{						set_LedBrightness( 0, 0, 0 );						set_BuzzerOnOff(false);					}					else if( ledSignalCounter < 0.6f )					{						set_LedBrightness( 0, 100, 0 );						set_BuzzerOnOff(true);					}					else					{						set_LedBrightness( 0, 0, 0 );						set_BuzzerOnOff(false);					}					break;				}												case LEDSignal_Err1:				{					if( ledSignalCounter > 1.0 )					{						ledSignalCounter = -1;						return;					}					set_BuzzerFreq(800);					set_BuzzerOnOff(true);					if( ledSignalCounter < 0.25f )						set_LedBrightness( 100, 0, 0 );					else if( ledSignalCounter < 0.5f )						set_LedBrightness( 0, 0, 0 );					else if( ledSignalCounter < 0.75f )						set_LedBrightness( 100, 0, 0 );					else						set_LedBrightness( 0, 0, 0 );					break;				}			}			ledSignalCounter += dt;			return;		}				static float counter = 0;		switch(ledmode)		{			/*正常模式*/				case LEDMode_Normal1:				{					if( counter > 2 )						counter = 0;					set_BuzzerOnOff(false);					if( counter < 1 )						set_LedBrightness( 0 , counter*100 , 0 );					else						set_LedBrightness( 0 , 200 - counter*100 , 0 );					break;				}			/*正常模式*/							/*飞行模式*/				case LEDMode_Flying1:				{					if( counter > 1.4 )						counter = 0;					set_BuzzerOnOff(false);					if( counter < 1 )						set_LedBrightness( 0 , counter*30 , 30-counter*30 );					else if( counter < 1.1 )						set_LedBrightness( 0 , 0 , 100 );					else if( counter < 1.2 )						set_LedBrightness( 0 , 0 , 0 );					else if( counter < 1.3 )						set_LedBrightness( 0 , 0 , 100 );					else						set_LedBrightness( 0 , 0 , 0 );					break;				}			/*飞行模式*/								/*处理中*/				case LEDMode_Processing1:				{					if( counter > 0.5 )						counter = 0;					set_BuzzerOnOff(false);					set_LedBrightness( 0 , 0 , counter*200 );					break;				}				case LEDMode_Processing2:				{					if( counter > 0.5 )						counter = 0;					set_BuzzerOnOff(false);					if( counter < 0.25 )						set_LedBrightness( 0 , 100-counter*400 , counter*400 );					else						set_LedBrightness( 0 , (counter-0.25)*400 , 100-(counter-0.25)*400 );					break;				}			/*处理中*/							default:			{	//用户手动控制				if(buzzerOn)					set_BuzzerFreq(buzzerFreq);				set_BuzzerOnOff(buzzerOn);				set_LedBrightness( ledR, ledG, ledB );				break;			}		}		counter += dt;	}/*声光提示*//*通信端口*/	//端口	static Port CommuPorts[ MAVLINK_COMM_NUM_BUFFERS ] = {0};	//发送消息列表	struct SDMsg	{		uint16_t counter;		uint16_t rate;	};	static map
    
      SDMessages[MAVLINK_COMM_NUM_BUFFERS];	static SemaphoreHandle_t SDMessagesMutex[MAVLINK_COMM_NUM_BUFFERS];		//在指定端口设置消息速率	bool SetMsgRate( uint8_t port_index, uint16_t Msg, uint16_t RateHz, double TIMEOUT )	{		if( port_index >= MAVLINK_COMM_NUM_BUFFERS )			return false;		if( Msg >= Mavlink_Send_Funcs_Count )			return false;		if( Mavlink_Send_Funcs[ Msg ] == 0 )			return false;				TickType_t TIMEOUT_Ticks;		if( TIMEOUT >= 0 )			TIMEOUT_Ticks = TIMEOUT*configTICK_RATE_HZ;		else			TIMEOUT_Ticks = portMAX_DELAY;		if( xSemaphoreTake( SDMessagesMutex[port_index], TIMEOUT_Ticks ) == pdTRUE )		{			uint16_t Rate = 100.0f / RateHz;			if( Rate==0 && RateHz!=0 )				Rate = 1;			map
     
      ::iterator it = SDMessages[port_index].find(Msg);			if( it == SDMessages[port_index].end() )			{	//无此消息 添加消息				if( Rate != 0 )				{					SDMsg sdmsg;					sdmsg.rate = Rate;					sdmsg.counter = 0;					SDMessages[port_index].insert( pair
      
       (Msg, sdmsg) );				}			}			else			{	//消息存在 更改速率				if( Rate != 0 )					it->second.rate = Rate;				else					SDMessages[port_index].erase(it);			}						xSemaphoreGive(SDMessagesMutex[port_index]);			return true;		}		return false;	}		//在指定端口发送消息列表	static bool sendParamListReset = false;	void sendParamList()	{		sendParamListReset = true;		ResetParametersIterator();	}		//注册端口用于协议通信	bool CommuPortRegister( Port port )	{		if( port.read == 0 )			return false;		if( port.write!=0 && (port.lock==0 || port.unlock==0) )			return false;				//寻找可用的位置		int8_t p_index = -1;		for( uint8_t i = 0 ; i < MAVLINK_COMM_NUM_BUFFERS ; ++i )		{			if( CommuPorts[ i ].read == 0 && CommuPorts[ i ].write == 0 )//maxi:那在还没绑定实际端口的时候，那个函数用的什么。				{//maxi:上面那个其实是看那个指针是否为零，没有绑定过的就是指向0的！！！！				p_index = i;				break;			}		}		//放满了		if( p_index < 0 )			return false;		//maxi:上面做的判断似乎是先看实际端口可不可用，再看逻辑端口可不可用，如果都可用下面把实际端口赋予逻辑端口。在这之前两者是没有连上的。		mavlink_init_chan( p_index );       //maxi:初始化逻辑端口，实际就是分配信号量		CommuPorts[ p_index ] = port;      //maxi：这个似乎关键？这是哪个i可用就把UART1赋给哪个cmmoPorts[i],这样就实现不同的i使用同一个UART1么？这句话是不是把虚拟端口和实际端口绑定起来了。		mavlink_set_proto_version( p_index , 1 );		SetMsgRate( p_index, MAVLINK_MSG_ID_ATTITUDE, 20 );		SetMsgRate( p_index, MAVLINK_MSG_ID_LOCAL_POSITION_NED, 20 );		SetMsgRate( p_index, MAVLINK_MSG_ID_GPS_RAW_INT, 5 );		SetMsgRate( p_index, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, 5 );		SetMsgRate( p_index, MAVLINK_MSG_ID_SYS_STATUS, 1 );		SetMsgRate( p_index, MAVLINK_MSG_ID_VFR_HUD, 2 );		return true;	}	//获取端口	const Port* get_Port( uint8_t port )	{		if( port < MAVLINK_COMM_NUM_BUFFERS )			return &CommuPorts[port];		else			return 0;	}/*通信端口*/	uint16_t mav_mode = MAV_MODE_PREFLIGHT;uint16_t mav_main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED;uint16_t mav_sub_mode = 0;static void Commulink_Server(void* pvParameters){	//初始化开始声音	sendLedSignal(LEDSignal_Start1);	//等待初始化完成	while( getInitializationCompleted() == false )	{		//刷新led声光提示		LEDRefresh(0.01f);		os_delay(0.01);	}	//心跳包计数器	uint16_t HeartBeat_counter = 0;		//准确周期延时	TickType_t xLastWakeTime;	xLastWakeTime = xTaskGetTickCount();	while(1)	{		//刷新led声光提示		LEDRefresh(0.01f);		bool sendHB = false;		if( ++HeartBeat_counter >= 100 )		{			HeartBeat_counter = 0;			sendHB = true;		}		for( uint8_t i = 0 ; i < MAVLINK_COMM_NUM_BUFFERS ; ++i )   //maxi:while(1)后面的i都是从这开始。		{	//遍历所有端口			mavlink_message_t msg_sd;			if( CommuPorts[i].write != 0 )//maxi:它这确定i的值也就是chan的值非常简单粗暴，就是看哪个端口有值在写入			{	//				if( sendHB )				{	//发送心跳包					if( mavlink_lock_chan(i,0.01) )					{						px4_custom_mode custom_mode;						custom_mode.main_mode = mav_main_mode;						custom_mode.sub_mode = mav_sub_mode;						mavlink_msg_heartbeat_pack_chan( 							1 ,	//system id							MAV_COMP_ID_AUTOPILOT1 ,	//component id							i	,	//chan							&msg_sd,							MAV_TYPE_QUADROTOR ,	//type							MAV_AUTOPILOT_PX4 ,	//autopilot							MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | mav_mode ,	//base mode							custom_mode.data ,	//custom mode							MAV_STATE_STANDBY	//sys status						);						mavlink_msg_to_send_buffer(CommuPorts[i].write, 																			 CommuPorts[i].lock,																			 CommuPorts[i].unlock,																			 &msg_sd, 0, 0.01);						mavlink_unlock_chan(i);					}				}								/*发送消息列表中的消息*/					#define MAX_SDMsgs 20					uint16_t sdmsgs[MAX_SDMsgs];					uint16_t sdmsgs_count = 0;					if( xSemaphoreTake( SDMessagesMutex[i], 0.01*configTICK_RATE_HZ ) == pdTRUE )					{											for( map
       
        ::iterator it = SDMessages[i].begin(); it != SDMessages[i].end(); ++it )						{							if( ++(it->second.counter) >= it->second.rate )							{								it->second.counter = 0;								if( sdmsgs_count < MAX_SDMsgs )									sdmsgs[sdmsgs_count++] = it->first;								else									break;							}						}						xSemaphoreGive(SDMessagesMutex[i]);					}					for( uint16_t k = 0; k < sdmsgs_count; ++k )					{						if( sdmsgs[k]
        
          0 ) { --RqMissionCounter[i]; if( (RqMissionCounter[i] & 0xf) == 0 ) { //超时发送请求 if( RqMissionInd[i] == 0 ) { //0号航点同时发送int和普通请求 const Port* port = get_Port(i); if( port->write != 0 ) { mavlink_message_t msg_sd; mavlink_lock_chan( i, 0.01 ); mavlink_msg_mission_request_int_pack_chan( 1 , //system id MAV_COMP_ID_AUTOPILOT1 , //component id i , //chan &msg_sd, RqMissiontarget_sysid[i] , //target system RqMissiontarget_compid[i] , //target component 0 , //seq MAV_MISSION_TYPE_MISSION //mission type ); mavlink_msg_to_send_buffer(port->write, port->lock, port->unlock, &msg_sd, 0, 0.01); mavlink_unlock_chan(i); mavlink_lock_chan( i, 0.01 ); mavlink_msg_mission_request_pack_chan( 1 , //system id MAV_COMP_ID_AUTOPILOT1 , //component id i , //chan &msg_sd, RqMissiontarget_sysid[i] , //target system RqMissiontarget_compid[i] , //target component 0 , //seq MAV_MISSION_TYPE_MISSION //mission type ); mavlink_msg_to_send_buffer(port->write, port->lock, port->unlock, &msg_sd, 0, 0.01); mavlink_unlock_chan(i); } } else if( RqMissionInt[i] ) { const Port* port = get_Port(i); if( port->write != 0 ) { mavlink_message_t msg_sd; mavlink_lock_chan( i, 0.01 ); mavlink_msg_mission_request_int_pack_chan( 1 , //system id MAV_COMP_ID_AUTOPILOT1 , //component id i , //chan &msg_sd, RqMissiontarget_sysid[i] , //target system RqMissiontarget_compid[i] , //target component RqMissionInd[i] , //seq MAV_MISSION_TYPE_MISSION //mission type ); mavlink_msg_to_send_buffer(port->write, port->lock, port->unlock, &msg_sd, 0, 0.01); mavlink_unlock_chan(i); } } else { const Port* port = get_Port(i); if( port->write != 0 ) { mavlink_message_t msg_sd; mavlink_lock_chan( i, 0.01 ); mavlink_msg_mission_request_pack_chan( 1 , //system id MAV_COMP_ID_AUTOPILOT1 , //component id i , //chan &msg_sd, RqMissiontarget_sysid[i] , //target system RqMissiontarget_compid[i] , //target component RqMissionInd[i] , //seq MAV_MISSION_TYPE_MISSION //mission type ); mavlink_msg_to_send_buffer(port->write, port->lock, port->unlock, &msg_sd, 0, 0.01); mavlink_unlock_chan(i); } } } } } /*发送航点请求*/ if( CommuPorts[i].read != 0 ) { //接收数据处理 //每次接收64个字节进行处理 mavlink_message_t msg; uint8_t buf[64]; //想想为什么是64个字节？mavlink消息包并不是64个字节。 uint8_t length; do { length = CommuPorts[i].read( buf, 64, 0, 0.01 ); //maxi:这实际是调用的UART的接收函数。 for( uint8_t k = 0; k < length; ++k ) { //消息解包 if( mavlink_parse_char( i, buf[k], &msg, NULL ) == MAVLINK_FRAMING_OK ) { //消息解包完成 /*********我自己加的部分*********/ vector3
         
           T265_position; //我看了下commulonk.cpp应该是包含了mavlink_msg_vision_position_estimate.h的，没有直接包含，应该间接包含了。因为在他下拉的头文件里看到了，而且这样编译没有报错。 T265_position.x=mavlink_msg_vision_position_estimate_get_x(&msg); T265_position.y=mavlink_msg_vision_position_estimate_get_y(&msg); T265_position.z=mavlink_msg_vision_position_estimate_get_z(&msg); //我应该还需要在这里做一下坐标系的变换，还有单位的变换，T265的坐标单位是米，ACfly单位是厘米。 //似乎x y z要放到它定义好的vector这种数据类型里面。 PositionSensorUpdatePosition( 9, T265_position, true); //maxi:后面的四个参数似乎都可以不写,都是默认参数似乎可以不写，其他的传感器似乎也是写着三个参数，第三个参数ture对应的bool available //maxi:我于2020.10.9 21:21写好了，而且编译没有错误！！！！！！！！个人独立完成。 /*********我自己加的部分********/ //如果消息处理函数存在 //处理消息 if( msg.msgid < Mavlink_RC_Process_Count ) { if( Mavlink_RC_Process[ msg.msgid ] != 0 ) Mavlink_RC_Process[ msg.msgid ]( i , &msg ); } } } }while( length > 10 ); } } if( sendParamListReset == false ) ParameterIteratorMoveNext(); else sendParamListReset = false; vTaskDelayUntil( &xLastWakeTime, 0.01*configTICK_RATE_HZ ); }}void init_Commulink(){ /********************我自己加的部分**************************/ //maxi:我这里可能要加个传感器注册函数。我可能还要把init_Commulink()这个函数加到传感器驱动初始化的文件frv_main.cpp里面。我看了下，它已经加进去了，看来我的理解是对的！！！mvalink就像一个传感器 //maxi:延时默认为0 了，TIMEOUT默认都是-1，优先级都为0 PositionSensorRegister( 9 ,\ Position_Sensor_Type_RangePositioning ,\ Position_Sensor_DataType_s_xyz ,\ Position_Sensor_frame_ENU ,\ 0,\ 0 ,\ 0 , \ -1 \ ); /***********************我自己加的部分*************************/ for( uint8_t i = 0; i < MAVLINK_COMM_NUM_BUFFERS; ++i ) { SDMessagesMutex[i] = xSemaphoreCreateMutex();//maxi:应该是给缓冲区建立MAVLINK_COMM_NUM_BUFFERS个互斥信号量，因为缓冲区就MAVLINK_COMM_NUM_BUFFERS个单位。 } xTaskCreate( Commulink_Server, "Commulink", 2048, NULL, SysPriority_UserTask, NULL); //maxi:果不其然，它他这创建了一个通信任务！似乎专门用来mavlink通信的，所以它通信的代码写在这里。}