打造自己的HelloDrone 无人机APP过程《4》

最新推荐文章于 2024-01-25 13:13:30 发布

魔城烟雨

最新推荐文章于 2024-01-25 13:13:30 发布

阅读量1.2k

点赞数 1

分类专栏： android学习文章标签： android

本文链接：https://blog.csdn.net/lixiaoweimashixiao/article/details/108959868

版权

android学习专栏收录该内容

15 篇文章 9 订阅

订阅专栏

摘要

本节主要讲解打造自己的HelloDrone 无人机APP过程《4》心跳包数据，欢迎批评指正！！！

1.首先看下飞控的心跳包数据定义

在飞控的common.xml文件中定义
在这里插入图片描述
我们补充下Mavlink相关信息

在mavlink消息帧里最重要的两个东西，一个是msgid；一个是payload，前者是payload中内容的编号，后者则存放了消息。消息有许多种类型，在官网的网页中中以蓝色的“#”加数字的方式来表示消息的编号如 “#0”（这样的表示方法应该是为了方便在网页中查找相应编号消息的定义）。在官网介绍网页里往下拉，大概拉到二分之一的位置处，开始出现“MAVLink Messages”的介绍，往下看是各种消息的数据组成说明。下面将以几个消息为例，讲解mavlink消息。

先以 #0 消息为例，这个消息叫心跳包（heartbeat）。它一般用来表明发出该消息的设备是活跃的，飞行器和地面站都会发出这个信号（一般以1Hz发送），地面站和飞行器会根据是否及时收到了心跳包来判断是否和飞行器或地面站失去了联系。

在这里插入图片描述
从图上可以看出，心跳包由6个数据组成，第一个是占一个字节的飞行器类型数据（type），这个数据表示了当前发消息的是什么飞行器，比如四旋翼，固定翼等等。type的取值如何与飞行器类型对应，这要在官方的mavlink消息介绍网页上找，位于网页开始出的数据枚举中。如下图所示：
在这里插入图片描述
这里只是一部分的类型，第一个是通用飞行器，对应的type数值是0；第二个是固定翼类型，对应的数值是1；第三个对应的是四旋翼，对应的数值是2.这个飞行器类型，其实对于发心跳包的地面站来说可能没什么意义（不同飞控对该消息的处理方法不同，至少刷了PX4固件的Pixhawk飞控对地面站发来的心跳包里的这个参数并不关心，如无特殊说明，之后所说的Pixhawk飞控都是指刷PX4固件的飞控），对于飞行器端来说代表了当前飞行器的类型，地面站可以根据这个参数来判断飞行器的类型并作出其他的反应。

第二个参数是自驾仪（即通常所说的飞控）类型，比如apm，ppz，Pixhawk等飞控，具体定义查找和之前查找飞行器类型时的方法一样。同样的，对于发送心跳包的飞行器来说代表了自己的飞控类性，对地面站发出的心跳包来说意义不大。

第三个参数是基本模式（base mode），是指飞控现在处在哪个基本模式，对于发心跳包的地面站来说没有意义，对于发送心跳包的飞控来说是有意义的。这个参数要看各个飞控自己的定义方式，mavlink介绍网页并不会给出具体的模式。在Pixhawk中基本模式可以分为使用用户模式（custom mode）还是基本模式（这里有点绕，其实是就是是否使用用户模式）。使用用户模式将在讲下个参数时说明，使用基本模式又会分为自动模式（auto），位置控制模式（posctl）和手动模式（manual）。一般情况下都会使用用户模式，普通用户不用关心这个参数。开发者在使用mavlink修改飞行器模式时需要注意基本模式的设置，具体请看PX4代码。

第四个参数是用户模式（custom mode），大概说一下Pixhawk的用户模式。以多轴为例。它分为主模式（main mode）和子模式（sub mode），两种模式组合在一起成为最终的模式，主模式分为3种，手动（manual），辅助（assist），自动（auto）。手动模式类似apm的姿态模式。在辅助模式中，又分为高度控制模式（altctl）和位置控制模式（posctl）两个子模式，高度控制模式就类似apm的定高模式，油门对应到飞行器高度控制上。位置模式控制飞行器相对地面的速度，油门和高度控制模式一样，yaw轴控制和手动模式一样。自动模式里又分为3个子模式，任务模式（mission），留待模式（loiter），返航模式（return），任务模式就是执行设定好的航点任务，留待模式就是gps悬停模式，返航模式就是直线返回home点并自动降落。在apm里这个参数貌似是没有用的，注意这个数据占了4个字节，在Pixhawk中，前两个字节（低位）是保留的，没有用，第三个字节是主模式，第四个字节是子模式。
第五个是系统状态（system status），查定义就好了，其中的standby状态在Pixhawk里就是还没解锁的状态，active状态就是已经解锁，准备起飞的状态。

第六个是mavlink版本（mavlink version），现在是“3”版本。

common.xml文件对应的C++代码

#pragma once
// MESSAGE HEARTBEAT PACKING

#define MAVLINK_MSG_ID_HEARTBEAT 0

MAVPACKED(
typedef struct __mavlink_heartbeat_t {
 uint32_t custom_mode; /*<  用户模式*/
 int32_t uav_id; /*<  飞控ID1*/
 int32_t uav_id1; /*< 飞控ID2*/
 int32_t uav_id2; /*<  飞控ID3*/
 uint8_t type; /*<  MAV 的类型(四轴quadrotor, 直升机helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)*/
 uint8_t autopilot; /*< 是自驾仪（即通常所说的飞控）类型 Autopilot type / class. defined in MAV_AUTOPILOT ENUM*/
 uint8_t base_mode; /*<  基本模式System mode bitfield, as defined by MAV_MODE_FLAG enum*/
 uint8_t drug_deprive; /*< 断药信息 System status flag, as defined drug deprivation*/
 uint8_t system_status; /*<  系统状态（system status），查定义就好了，其中的standby状态在Pixhawk里就是还没解锁的状态，active状态就是已经解锁，准备起飞的状态System status flag, as defined by MAV_STATE enum*/
 uint8_t mavlink_version; /*<  MAVLink version版本, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version*/
}) mavlink_heartbeat_t;

#define MAVLINK_MSG_ID_HEARTBEAT_LEN 22  //心跳包的数据长度
#define MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN 22
#define MAVLINK_MSG_ID_0_LEN 22
#define MAVLINK_MSG_ID_0_MIN_LEN 22

#define MAVLINK_MSG_ID_HEARTBEAT_CRC 139 //校验
#define MAVLINK_MSG_ID_0_CRC 139



#if MAVLINK_COMMAND_24BIT
#define MAVLINK_MESSAGE_INFO_HEARTBEAT { \
    0, \
    "HEARTBEAT", \
    10, \
    {  { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_heartbeat_t, type) }, \
         { "autopilot", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_heartbeat_t, autopilot) }, \
         { "base_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_heartbeat_t, base_mode) }, \
         { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_heartbeat_t, custom_mode) }, \
         { "uav_id", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_heartbeat_t, uav_id) }, \
         { "uav_id1", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_heartbeat_t, uav_id1) }, \
         { "uav_id2", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_heartbeat_t, uav_id2) }, \
         { "drug_deprive", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_heartbeat_t, drug_deprive) }, \
         { "system_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_heartbeat_t, system_status) }, \
         { "mavlink_version", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_heartbeat_t, mavlink_version) }, \
         } \
}
#else
#define MAVLINK_MESSAGE_INFO_HEARTBEAT { \
    "HEARTBEAT", \
    10, \
    {  { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_heartbeat_t, type) }, \
         { "autopilot", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_heartbeat_t, autopilot) }, \
         { "base_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_heartbeat_t, base_mode) }, \
         { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_heartbeat_t, custom_mode) }, \
         { "uav_id", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_heartbeat_t, uav_id) }, \
         { "uav_id1", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_heartbeat_t, uav_id1) }, \
         { "uav_id2", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_heartbeat_t, uav_id2) }, \
         { "drug_deprive", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_heartbeat_t, drug_deprive) }, \
         { "system_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_heartbeat_t, system_status) }, \
         { "mavlink_version", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_heartbeat_t, mavlink_version) }, \
         } \
}
#endif

/**
 * @brief Pack a heartbeat message
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param msg The MAVLink message to compress the data into
 *
 * @param type  Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
 * @param autopilot  Autopilot type / class. defined in MAV_AUTOPILOT ENUM
 * @param base_mode  System mode bitfield, as defined by MAV_MODE_FLAG enum
 * @param custom_mode  A bitfield for use for autopilot-specific flags
 * @param uav_id  A bitfield for use for autopilot-specific flags
 * @param uav_id1  A bitfield for use for autopilot-specific flags
 * @param uav_id2  A bitfield for use for autopilot-specific flags
 * @param drug_deprive  System status flag, as defined drug deprivation
 * @param system_status  System status flag, as defined by MAV_STATE enum
 * @return length of the message in bytes (excluding serial stream start sign)
 */
static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
                               uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, int32_t uav_id, int32_t uav_id1, int32_t uav_id2, uint8_t drug_deprive, 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_int32_t(buf, 4, uav_id);
    _mav_put_int32_t(buf, 8, uav_id1);
    _mav_put_int32_t(buf, 12, uav_id2);
    _mav_put_uint8_t(buf, 16, type);
    _mav_put_uint8_t(buf, 17, autopilot);
    _mav_put_uint8_t(buf, 18, base_mode);
    _mav_put_uint8_t(buf, 19, drug_deprive);
    _mav_put_uint8_t(buf, 20, system_status);
    _mav_put_uint8_t(buf, 21, 3);

        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
#else
    mavlink_heartbeat_t packet;
    packet.custom_mode = custom_mode;
    packet.uav_id = uav_id;
    packet.uav_id1 = uav_id1;
    packet.uav_id2 = uav_id2;
    packet.type = type;
    packet.autopilot = autopilot;
    packet.base_mode = base_mode;
    packet.drug_deprive = drug_deprive;
    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(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
}

/**
 * @brief Pack a heartbeat message on a channel
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param chan The MAVLink channel this message will be sent over
 * @param msg The MAVLink message to compress the data into
 * @param type  Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
 * @param autopilot  Autopilot type / class. defined in MAV_AUTOPILOT ENUM
 * @param base_mode  System mode bitfield, as defined by MAV_MODE_FLAG enum
 * @param custom_mode  A bitfield for use for autopilot-specific flags
 * @param uav_id  A bitfield for use for autopilot-specific flags
 * @param uav_id1  A bitfield for use for autopilot-specific flags
 * @param uav_id2  A bitfield for use for autopilot-specific flags
 * @param drug_deprive  System status flag, as defined drug deprivation
 * @param system_status  System status flag, as defined by MAV_STATE enum
 * @return length of the message in bytes (excluding serial stream start sign)
 */
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,int32_t uav_id,int32_t uav_id1,int32_t uav_id2,uint8_t drug_deprive,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_int32_t(buf, 4, uav_id);
    _mav_put_int32_t(buf, 8, uav_id1);
    _mav_put_int32_t(buf, 12, uav_id2);
    _mav_put_uint8_t(buf, 16, type);
    _mav_put_uint8_t(buf, 17, autopilot);
    _mav_put_uint8_t(buf, 18, base_mode);
    _mav_put_uint8_t(buf, 19, drug_deprive);
    _mav_put_uint8_t(buf, 20, system_status);
    _mav_put_uint8_t(buf, 21, 3);

        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN);
#else
    mavlink_heartbeat_t packet;
    packet.custom_mode = custom_mode;
    packet.uav_id = uav_id;
    packet.uav_id1 = uav_id1;
    packet.uav_id2 = uav_id2;
    packet.type = type;
    packet.autopilot = autopilot;
    packet.base_mode = base_mode;
    packet.drug_deprive = drug_deprive;
    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);
}

/**
 * @brief Encode a heartbeat struct
 *
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param msg The MAVLink message to compress the data into
 * @param heartbeat C-struct to read the message contents from
 */
static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat)
{
    return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->uav_id, heartbeat->uav_id1, heartbeat->uav_id2, heartbeat->drug_deprive, heartbeat->system_status);
}

/**
 * @brief Encode a heartbeat struct on a channel
 *
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param chan The MAVLink channel this message will be sent over
 * @param msg The MAVLink message to compress the data into
 * @param heartbeat C-struct to read the message contents from
 */
static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat)
{
    return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->uav_id, heartbeat->uav_id1, heartbeat->uav_id2, heartbeat->drug_deprive, heartbeat->system_status);
}

/**
 * @brief Send a heartbeat message
 * @param chan MAVLink channel to send the message
 *
 * @param type  Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
 * @param autopilot  Autopilot type / class. defined in MAV_AUTOPILOT ENUM
 * @param base_mode  System mode bitfield, as defined by MAV_MODE_FLAG enum
 * @param custom_mode  A bitfield for use for autopilot-specific flags
 * @param uav_id  A bitfield for use for autopilot-specific flags
 * @param uav_id1  A bitfield for use for autopilot-specific flags
 * @param uav_id2  A bitfield for use for autopilot-specific flags
 * @param drug_deprive  System status flag, as defined drug deprivation
 * @param system_status  System status flag, as defined by MAV_STATE enum
 */
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS

static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, int32_t uav_id, int32_t uav_id1, int32_t uav_id2, uint8_t drug_deprive, 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_int32_t(buf, 4, uav_id);
    _mav_put_int32_t(buf, 8, uav_id1);
    _mav_put_int32_t(buf, 12, uav_id2);
    _mav_put_uint8_t(buf, 16, type);
    _mav_put_uint8_t(buf, 17, autopilot);
    _mav_put_uint8_t(buf, 18, base_mode);
    _mav_put_uint8_t(buf, 19, drug_deprive);
    _mav_put_uint8_t(buf, 20, system_status);
    _mav_put_uint8_t(buf, 21, 3);

    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
#else
    mavlink_heartbeat_t packet;
    packet.custom_mode = custom_mode;
    packet.uav_id = uav_id;
    packet.uav_id1 = uav_id1;
    packet.uav_id2 = uav_id2;
    packet.type = type;
    packet.autopilot = autopilot;
    packet.base_mode = base_mode;
    packet.drug_deprive = drug_deprive;
    packet.system_status = system_status;
    packet.mavlink_version = 3;

    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
#endif
}

/**
 * @brief Send a heartbeat message
 * @param chan MAVLink channel to send the message
 * @param struct The MAVLink struct to serialize
 */
static inline void mavlink_msg_heartbeat_send_struct(mavlink_channel_t chan, const mavlink_heartbeat_t* heartbeat)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
    mavlink_msg_heartbeat_send(chan, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->uav_id, heartbeat->uav_id1, heartbeat->uav_id2, heartbeat->drug_deprive, heartbeat->system_status);
#else
    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)heartbeat, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
#endif
}

#if MAVLINK_MSG_ID_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN
/*
  This varient of _send() can be used to save stack space by re-using
  memory from the receive buffer.  The caller provides a
  mavlink_message_t which is the size of a full mavlink message. This
  is usually the receive buffer for the channel, and allows a reply to an
  incoming message with minimum stack space usage.
 */
static inline void mavlink_msg_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, int32_t uav_id, int32_t uav_id1, int32_t uav_id2, uint8_t drug_deprive, uint8_t system_status)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
    char *buf = (char *)msgbuf;
    _mav_put_uint32_t(buf, 0, custom_mode);
    _mav_put_int32_t(buf, 4, uav_id);
    _mav_put_int32_t(buf, 8, uav_id1);
    _mav_put_int32_t(buf, 12, uav_id2);
    _mav_put_uint8_t(buf, 16, type);
    _mav_put_uint8_t(buf, 17, autopilot);
    _mav_put_uint8_t(buf, 18, base_mode);
    _mav_put_uint8_t(buf, 19, drug_deprive);
    _mav_put_uint8_t(buf, 20, system_status);
    _mav_put_uint8_t(buf, 21, 3);

    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
#else
    mavlink_heartbeat_t *packet = (mavlink_heartbeat_t *)msgbuf;
    packet->custom_mode = custom_mode;
    packet->uav_id = uav_id;
    packet->uav_id1 = uav_id1;
    packet->uav_id2 = uav_id2;
    packet->type = type;
    packet->autopilot = autopilot;
    packet->base_mode = base_mode;
    packet->drug_deprive = drug_deprive;
    packet->system_status = system_status;
    packet->mavlink_version = 3;

    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
#endif
}
#endif

#endif

// MESSAGE HEARTBEAT UNPACKING


/**
 * @brief Get field type from heartbeat message
 *
 * @return  Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
 */
static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg)
{
    return _MAV_RETURN_uint8_t(msg,  16);
}

/**
 * @brief Get field autopilot from heartbeat message
 *
 * @return  Autopilot type / class. defined in MAV_AUTOPILOT ENUM
 */
static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg)
{
    return _MAV_RETURN_uint8_t(msg,  17);
}

/**
 * @brief Get field base_mode from heartbeat message
 *
 * @return  System mode bitfield, as defined by MAV_MODE_FLAG enum
 */
static inline uint8_t mavlink_msg_heartbeat_get_base_mode(const mavlink_message_t* msg)
{
    return _MAV_RETURN_uint8_t(msg,  18);
}

/**
 * @brief Get field custom_mode from heartbeat message
 *
 * @return  A bitfield for use for autopilot-specific flags
 */
static inline uint32_t mavlink_msg_heartbeat_get_custom_mode(const mavlink_message_t* msg)
{
    return _MAV_RETURN_uint32_t(msg,  0);
}

/**
 * @brief Get field uav_id from heartbeat message
 *
 * @return  A bitfield for use for autopilot-specific flags
 */
static inline int32_t mavlink_msg_heartbeat_get_uav_id(const mavlink_message_t* msg)
{
    return _MAV_RETURN_int32_t(msg,  4);
}

/**
 * @brief Get field uav_id1 from heartbeat message
 *
 * @return  A bitfield for use for autopilot-specific flags
 */
static inline int32_t mavlink_msg_heartbeat_get_uav_id1(const mavlink_message_t* msg)
{
    return _MAV_RETURN_int32_t(msg,  8);
}

/**
 * @brief Get field uav_id2 from heartbeat message
 *
 * @return  A bitfield for use for autopilot-specific flags
 */
static inline int32_t mavlink_msg_heartbeat_get_uav_id2(const mavlink_message_t* msg)
{
    return _MAV_RETURN_int32_t(msg,  12);
}

/**
 * @brief Get field drug_deprive from heartbeat message
 *
 * @return  System status flag, as defined drug deprivation
 */
static inline uint8_t mavlink_msg_heartbeat_get_drug_deprive(const mavlink_message_t* msg)
{
    return _MAV_RETURN_uint8_t(msg,  19);
}

/**
 * @brief Get field system_status from heartbeat message
 *
 * @return  System status flag, as defined by MAV_STATE enum
 */
static inline uint8_t mavlink_msg_heartbeat_get_system_status(const mavlink_message_t* msg)
{
    return _MAV_RETURN_uint8_t(msg,  20);
}

/**
 * @brief Get field mavlink_version from heartbeat message
 *
 * @return  MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version
 */
static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg)
{
    return _MAV_RETURN_uint8_t(msg,  21);
}

/**
 * @brief Decode a heartbeat message into a struct
 *
 * @param msg The message to decode
 * @param heartbeat C-struct to decode the message contents into
 */
static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
    heartbeat->custom_mode = mavlink_msg_heartbeat_get_custom_mode(msg);
    heartbeat->uav_id = mavlink_msg_heartbeat_get_uav_id(msg);
    heartbeat->uav_id1 = mavlink_msg_heartbeat_get_uav_id1(msg);
    heartbeat->uav_id2 = mavlink_msg_heartbeat_get_uav_id2(msg);
    heartbeat->type = mavlink_msg_heartbeat_get_type(msg);
    heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg);
    heartbeat->base_mode = mavlink_msg_heartbeat_get_base_mode(msg);
    heartbeat->drug_deprive = mavlink_msg_heartbeat_get_drug_deprive(msg);
    heartbeat->system_status = mavlink_msg_heartbeat_get_system_status(msg);
    heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg);
#else
        uint8_t len = msg->len < MAVLINK_MSG_ID_HEARTBEAT_LEN? msg->len : MAVLINK_MSG_ID_HEARTBEAT_LEN;
        memset(heartbeat, 0, MAVLINK_MSG_ID_HEARTBEAT_LEN);
    memcpy(heartbeat, _MAV_PAYLOAD(msg), len);
#endif
}

2.我们看下APP心跳包数据定义

/* AUTO-GENERATED FILE.  DO NOT MODIFY.
 *
 * This class was automatically generated by the
 * java mavlink generator tool. It should not be modified by hand.
 */

// MESSAGE HEARTBEAT PACKING
package com.MAVLink.common;
import com.MAVLink.MAVLinkPacket;
import com.MAVLink.Messages.MAVLinkMessage;
import com.MAVLink.Messages.MAVLinkPayload;



/**
 * heartbeat--心跳消息显示系统存在并正在响应。
 * MAV和自动驾驶仪硬件的类型允许接收系统适当地处理来自该系统的进一步消息（例如，根据自动驾驶仪布置用户界面）。
 * The heartbeat message shows that a system is present and responding.
 * The type of the MAV and Autopilot hardware allow the receiving system to
 * treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot).
 */
public class msg_heartbeat extends MAVLinkMessage{

    public static final int MAVLINK_MSG_ID_HEARTBEAT = 0;
    public static final int MAVLINK_MSG_LENGTH = 22; //心跳包长度
    private static final long serialVersionUID = MAVLINK_MSG_ID_HEARTBEAT; //版本ID



    /**
     * A bitfield for use for autopilot-specific flags.
     */
    public long custom_mode; //用户模式
    public int uav_id;       //飞控ID
    public int uav_id1;      //飞控ID
    public int uav_id2;      //飞控ID
    /**
     * Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
     */
    public short type;//无人机类型

    /**
     * Autopilot type / class. defined in MAV_AUTOPILOT ENUM
     */
    public short autopilot;//自动驾驶仪类型

    /**
     * System mode bitfield, see MAV_MODE_FLAG ENUM in mavlink/include/mavlink_types.h
     */
    public short base_mode; //基本模式
    //断药信息
    public byte drug_deprive;

    /**
     * System status flag, see MAV_STATE ENUM
     */
    public short system_status; //系统状态

    /**
     * MAVLink版本---- version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version
     */
    public short mavlink_version;


    /**
     * 为这种类型的消息生成mavlink消息的有效负载
     * @return
     */
    public MAVLinkPacket pack(){
        MAVLinkPacket packet = new MAVLinkPacket(MAVLINK_MSG_LENGTH);
        packet.sysid = 255;
        packet.compid = 190;
        packet.msgid = MAVLINK_MSG_ID_HEARTBEAT;

        packet.payload.putUnsignedInt(custom_mode);
        packet.payload.putInt(uav_id);
        packet.payload.putInt(uav_id1);
        packet.payload.putInt(uav_id2);
        packet.payload.putUnsignedByte(type);

        packet.payload.putUnsignedByte(autopilot);

        packet.payload.putUnsignedByte(base_mode);
        packet.payload.putByte(drug_deprive);
        packet.payload.putUnsignedByte(system_status);

        packet.payload.putUnsignedByte(mavlink_version);

        return packet;
    }

    /**
     * 将心跳信号消息解码到此类字段中
     *
     * @param payload The message to decode
     */
    public void unpack(MAVLinkPayload payload) {
        payload.resetIndex();

        this.custom_mode = payload.getUnsignedInt();
        this.uav_id = payload.getInt();
        this.uav_id1 = payload.getInt();
        this.uav_id2 = payload.getInt();
        this.type = payload.getUnsignedByte();

        this.autopilot = payload.getUnsignedByte();

        this.base_mode = payload.getUnsignedByte();
        this.drug_deprive = payload.getByte();
        this.system_status = payload.getUnsignedByte();

        this.mavlink_version = payload.getUnsignedByte();

    }

    /**
     * 构造函数，只需初始化msgid
     */
    public msg_heartbeat(){
        msgid = MAVLINK_MSG_ID_HEARTBEAT;
    }

    /**
     * 构造函数，使用来自mavlink包的有效负载初始化消息
     */
    public msg_heartbeat(MAVLinkPacket mavLinkPacket){
        this.sysid = mavLinkPacket.sysid;
        this.compid = mavLinkPacket.compid;
        this.msgid = MAVLINK_MSG_ID_HEARTBEAT;

        unpack(mavLinkPacket.payload);
    }


    /**
     * 返回包含消息名称和数据的字符串
     */
    public String toString(){
        return "MAVLINK_MSG_ID_HEARTBEAT - " +
                "sysid:"+sysid+
                " compid:"+compid+" " +
                "custom_mode:"+custom_mode+
                "uav_id:"+uav_id+
                "uav_id1:"+uav_id1+
                "uav_id2:"+uav_id2+
                " type:"+type+
                " autopilot:" +autopilot+
                " base_mode:"+base_mode+
                "drug_deprive:"+drug_deprive+
                " system_status:"+system_status+
                " mavlink_version:"+mavlink_version+
                "";
    }
}

可以看出飞控和APP的协议必须一致才能够建立连接。
还有一个需要注意的地方就是心跳包的CRC校验，这个飞控和APP必须保持一致 在这里插入图片描述

3.APP获取飞控心跳包数据并处理

在这里插入图片描述

1. onHeartbeat(message);

    private void onHeartbeat(MAVLinkMessage msg) {
        //处理心跳信息
        Log.i("zqj"," heart msg:"+msg);
        heartbeat.onHeartbeat(msg);
    }

打印输出的信息：
在这里插入图片描述

heart msg:MAVLINK_MSG_ID_HEARTBEAT - sysid:1 compid:1 custom_mode:0uav_id:1900591uav_id1:825774358uav_id2:892549175 type:2 autopilot:3 base_mode:81drug_deprive:0 system_status:4 mavlink_version:3

内部代码

    public void onHeartbeat(MAVLinkMessage msg) {
        msg_heartbeat heartBeatMsg = msg instanceof msg_heartbeat ? (msg_heartbeat) msg : null;
        Log.i("zqj"," heartBeatMsg:"+heartBeatMsg);
        if(heartBeatMsg != null){
            sysid  = validateToUnsignedByteRange(msg.sysid);
            compid = validateToUnsignedByteRange(msg.compid);
            mMavlinkVersion = heartBeatMsg.mavlink_version;
            Log.i("zqj"," sysid:"+sysid);
            Log.i("zqj"," compid:"+compid);
            Log.i("zqj"," mMavlinkVersion:"+mMavlinkVersion);
        }
        Log.i("zqj"," heartbeatState:"+heartbeatState);
        //对于这个heartbeatState第一次是等于0，后面等于2，如果丢失会变成1
        switch (heartbeatState) {
            case FIRST_HEARTBEAT:
                if(heartBeatMsg != null) {
                    Timber.i("Received first heartbeat.");

                    heartbeatState = NORMAL_HEARTBEAT;
                    restartWatchdog(HEARTBEAT_NORMAL_TIMEOUT);

                    myDrone.notifyDroneEvent(DroneEventsType.HEARTBEAT_FIRST);
                }
                break;

            case LOST_HEARTBEAT:
                myDrone.notifyDroneEvent(DroneEventsType.HEARTBEAT_RESTORED);
            // FALL THROUGH

            default:
                heartbeatState = NORMAL_HEARTBEAT;
                restartWatchdog(HEARTBEAT_NORMAL_TIMEOUT);
                break;
        }
    }

log打印信息
在这里插入图片描述

2. processHeartbeat(msg_heart);

            case msg_heartbeat.MAVLINK_MSG_ID_HEARTBEAT:
                msg_heartbeat msg_heart = (msg_heartbeat) message;
                processHeartbeat(msg_heart);
                break;

    /**
     * 处理心跳信息
     * @param msg_heart
     */
    private void processHeartbeat(msg_heartbeat msg_heart) {
        Log.i("zqj","msg_heart.type ："+msg_heart.type );
        Log.i("zqj","msg_heart ："+msg_heart );
        setType(msg_heart.type);
        //检测是否在飞行
        checkIfFlying(msg_heart);
        //检测解锁状态，安全状态
        processState(msg_heart);
        //处理当前飞行器模式
        Log.i("zqj","mode ：" );
        processVehicleMode(msg_heart);
    }

日志打印信息
在这里插入图片描述

1.判断飞机是否在飞行

    /**
     * 检测是否在飞行
     * @param msg_heart
     */
    private void checkIfFlying(msg_heartbeat msg_heart) {
        short systemStatus = msg_heart.system_status;
        boolean wasFlying = state.isFlying();

        boolean isFlying = systemStatus == MAV_STATE.MAV_STATE_ACTIVE
                || (wasFlying
                && (systemStatus == MAV_STATE.MAV_STATE_CRITICAL || systemStatus == MAV_STATE.MAV_STATE_EMERGENCY));

        state.setIsFlying(isFlying);
    }

2.判断解锁状态，安全开关

    private void processState(msg_heartbeat msg_heart) {
        checkArmState(msg_heart);
        checkFailsafe(msg_heart);
    }

    private void checkArmState(msg_heartbeat msg_heart) {
        state.setArmed(
                (msg_heart.base_mode & MAV_MODE_FLAG.MAV_MODE_FLAG_SAFETY_ARMED) == MAV_MODE_FLAG.MAV_MODE_FLAG_SAFETY_ARMED);
    }

    private void checkFailsafe(msg_heartbeat msg_heart) {
        boolean failsafe2 = msg_heart.system_status == MAV_STATE.MAV_STATE_CRITICAL
                || msg_heart.system_status == MAV_STATE.MAV_STATE_EMERGENCY;

        if (failsafe2) {
            state.repeatWarning();
        }
    }

3.设置模式

    private void processVehicleMode(msg_heartbeat msg_heart) {
        ApmModes newMode = ApmModes.getMode(msg_heart.custom_mode, getType());
        Log.i("zqj","newMode ："+newMode );
        state.setMode(newMode);
    }

魔城烟雨

关注

1
点赞
踩
1

收藏

觉得还不错? 一键收藏
打赏
2
评论
打造自己的HelloDrone 无人机APP过程《4》

目录文章目录目录摘要1.首先看下飞控的心跳包数据定义2.我们看下APP心跳包数据定义3.APP获取飞控心跳包数据并处理1. onHeartbeat(message);2. processHeartbeat(msg_heart);摘要本节主要讲解打造自己的HelloDrone 无人机APP过程《4》心跳包数据，欢迎批评指正！！！1.首先看下飞控的心跳包数据定义在飞控的common.xml文件中定义我们补充下Mavlink相关信息在mavlink消息帧里最重要的两个东西，一个是msgid
复制链接

扫一扫