mavlink version

mavlink本身提供了一种版本号校验的方式,开源的代码生成器对此做了处理的,在xml把字段类型定义成uint8_t_mavlink_version,生成的时候应该就是直接取xml的version,交互双方可以直接根据此字段校验双方版本。

示例

在官方提供的common.xml文件中,对HEARTBEAT消息做了如下定义

<message id="0" name="HEARTBEAT">
  <description>The heartbeat message shows that a system or component is present and responding. The type and autopilot fields (along with the message component id), allow the receiving system to treat further messages from this system appropriately (e.g. by laying out the user interface based on the autopilot). This microservice is documented at https://mavlink.io/en/services/heartbeat.html</description>
  <field type="uint8_t" name="type" enum="MAV_TYPE">Type of the system (quadrotor, helicopter, etc.). Components use the same type as their associated system.</field>
  <field type="uint8_t" name="autopilot" enum="MAV_AUTOPILOT">Autopilot type / class.</field>
  <field type="uint8_t" name="base_mode" enum="MAV_MODE_FLAG" display="bitmask">System mode bitmap.</field>
  <field type="uint32_t" name="custom_mode">A bitfield for use for autopilot-specific flags</field>
  <field type="uint8_t" name="system_status" enum="MAV_STATE">System status flag.</field>
  <field type="uint8_t_mavlink_version" name="mavlink_version">MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version</field>
</message>

最后一个字段mavlink_version的类型为uint8_t_mavlink_version

查看生成的mavlink_msg_heartbeat.h文件,对heartbeat消息的编码函数如下:

static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
                               mavlink_message_t* msg,
                                   uint8_t type,uint8_t autopilot,uint8_t base_mode,uint32_t custom_mode,uint8_t system_status)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
    char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN];
    _mav_put_uint32_t(buf, 0, custom_mode);
    _mav_put_uint8_t(buf, 4, type);
    _mav_put_uint8_t(buf, 5, autopilot);
    _mav_put_uint8_t(buf, 6, base_mode);
    _mav_put_uint8_t(buf, 7, system_status);
    _mav_put_uint8_t(buf, 8, 3);

        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
#else
    mavlink_heartbeat_t packet;
    packet.custom_mode = custom_mode;
    packet.type = type;
    packet.autopilot = autopilot;
    packet.base_mode = base_mode;
    packet.system_status = system_status;
    packet.mavlink_version = 3;

        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN);
#endif

    msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
}

其对字段mavlink_version赋值为常量3,而常量3则是common.xml定义的version标签值

<?xml version="1.0"?>
<mavlink>
  <version>3</version>
  <dialect>0</dialect>
  ...
</mavlink>

生成器对此的实现

mavparse.py中定义了类 MAVField

class MAVField(object):
    def __init__(self, name, type, print_format, xml, description='', enum='', display='', units='', instance=False):
        self.name = name
        ...
        self.omit_arg = False
        self.const_value = None
        ...
        lengths = {
        'float'    : 4,
        'double'   : 8,
        'char'     : 1,
        'int8_t'   : 1,
        'uint8_t'  : 1,
        'uint8_t_mavlink_version'  : 1,
        'int16_t'  : 2,
        'uint16_t' : 2,
        'int32_t'  : 4,
        'uint32_t' : 4,
        'int64_t'  : 8,
        'uint64_t' : 8,
        }

        if type=='uint8_t_mavlink_version':
            type = 'uint8_t'
            self.omit_arg = True
            self.const_value = xml.version
		 ...

在mavgen_c.py中,函数generate_one有如下实现:

def generate_one(basename, xml):
    '''generate headers for one XML file'''
	...
	# cope with uint8_t_mavlink_version
    for m in xml.message:
		...
		for f in m.fields:
				if not f.omit_arg:
					m.arg_fields.append(f)
					f.putname = f.name
				else:
					f.putname = f.const_value

可见,uint8_t_mavlink_version类型即为uint8_t,并且,当为此类型时,putname使用的是const_value,也即xml.version,否则即为name(字段名称)

posted @ 2024-03-10 16:10  流翎  阅读(46)  评论(0编辑  收藏  举报