ardupilot之mavlink消息--从飞控发出--单向

飞控采用mavlink消息进行数据的传输。普遍说法是，现有的mavlink消息几乎已经涵盖了所有你的能想象到的内容，完全可以覆盖多处需求。 无奈科研总是要定义一些新鲜玩意 ，所以总是有无法完全满足需求，需要自定义消息的时候。

1. mavlink 消息的创建，首先在common.xml文件中找到对应的message部分，在common.xml文件的最后部分， 对应复制一个message消息，修改对应的消息ID，消息名称，消息内容，然后编译即可，生成对应的.h文件，对应的.h文件在 build/XXX/libraries/GCS_Mavlink/.../common/下面，对应的默认生成名字为 mavlink_msg_XXX.h
2. 对应默认生成的 mavlink_msg_XXX.h 中主要包含有如下几个函数
   mavlink_msg_XXX_pack( )
   mavlink_msg_XXX_pack_chan( )
   mavlink_msg_XXX_encode( )
   mavlink_msg_XXX_encode_chan( )
   mavlink_msg_XXX_send( )
   mavlink_msg_XXX_send_struct( )
   其中常用的是mavlink_msg_XXX_send()。
3. 但是地面站中是无法显示自己定义的mavlink消息的，初步认为地面站要显示自己定义的mavlink消息，需要修改地面站源码。那么只能修改现有的mavlink消息了…
4. 一个比较重要的函数是
   在文件gcs_common.h下面 GCS_MAVLINK::try_send_message( )
   switch(id){
   添加 case XXX: 以及相应的语句. 但是在case中添加自定义的消息却不会被发送出去。暂时没有找到相应原因。因此，综合上述3，4，选择修改现有的Mavlink消息
5. 考虑到要发送的消息中含有float数据类型，选择修改attitude_quaternion消息， 如下图所示:
   
   并且将上述函数同attitude 的mavlink消息放在同一个case 下面。从mission planner地面站中可以看到对应的消息已经出现
   
6. 由于dronekit 自带的vehicle属性中没有对 ATTITUDE_QUATERNION 消息的订阅，所以需要参照 dronekit 官网完成对 该 mavlink消息的添加监听工作。其对应的代码如下：

import time

from dronekit import Vehicle, connect
import numpy as np

class RawQuaternion(object):
    def __init__(self, time_boot_us=None, q1=None, q2=None, q3=None, q4=None, rollspeed=None, pitchspeed=None, yawspeed=None, off=None ):
        self.time_boot_us = time_boot_us
        self.q1 = q1
        self.q2 = q2
        self.q3 = q3
        self.q4 = q4
        self.rollspeed = rollspeed
        self.pitchspeed = pitchspeed
        self.yawspeed = yawspeed
        self.off = off

class Myvehicle(Vehicle):
    def __init__(self, *args):
        super(Myvehicle, self).__init__(*args)

        self._raw_quaternion = RawQuaternion()
        @self.on_message('ATTITUDE_QUATERNION')
        def listener(self, name, message):
            self._raw_quaternion.time_boot_us = message.time_boot_ms
            self._raw_quaternion.q1 = message.q1
            self._raw_quaternion.q2 = message.q2
            self._raw_quaternion.q3 = message.q3
            self._raw_quaternion.q4 = message.q4
            self._raw_quaternion.rollspeed = message.rollspeed
            self._raw_quaternion.pitchspeed = message.pitchspeed
            self._raw_quaternion.yawspeed = message.yawspeed
            self._raw_quaternion.off = message.repr_offset_q
            self.notify_attribute_listeners('attitude_quaternion',self._raw_quaternion)

    @property
    def raw_quaternion(self):
        quaternion_np = np.array([self._raw_quaternion.q1,self._raw_quaternion.q2,self._raw_quaternion.q3,self._raw_quaternion.q4])
        return quaternion_np

    @property
    def raw_anglespeed(self):
        anglespeed_np = np.array([self._raw_quaternion.rollspeed, self._raw_quaternion.pitchspeed, self._raw_quaternion.yawspeed])
        return anglespeed_np

vehicle1=connect('127.0.0.1:14551',wait_ready=True, vehicle_class= Myvehicle)
while True:
    time.sleep(0.5)
    print(vehicle1.raw_quaternion, end="")
    print(vehicle1.raw_anglespeed)

其代码显示结果如下：

至此可以在python中显示自己需要发送的数据了。