mavlink中的base_mode和custom_mode

诗筱涵

于 2024-06-16 11:17:47 发布

阅读量858

点赞数 11

分类专栏：无人机-mavros控制PX4 offboard 无人机-mavlink 无人机-pixhawk-PX4 文章标签： mavlink

本文链接：https://blog.csdn.net/sinat_16643223/article/details/139717355

版权

无人机-pixhawk-PX4 同时被 3 个专栏收录

167 篇文章 104 订阅

订阅专栏

无人机-mavros控制PX4 offboard

138 篇文章 88 订阅

订阅专栏

无人机-mavlink

46 篇文章 32 订阅

订阅专栏

base_mode和custom_mode是mavlink心跳包中很重要的组成部分，用来反应无人机当前的模式状态。QGC地面站如何知道无人机当前是否解锁，处于什么飞行模式，就是依靠心跳包里的base_mode和custom_mode，/mavros/state话题里的一些状态项的值，也是来自于心跳包里的base_mode和custom_mode。同时我们用QGC进行无人机的模式切换，QGC发送的是SET_MODE这个mavlink消息给飞控，SET_MODE(#11) mavlink消息里面其实主要就是base_mode和custom_mode这两项。
HEARTBEAT(#0) mavlink消息
http://mavlink.io/zh/messages/common.html

输入图片说明

SET_MODE(#11) mavlink消息
http://mavlink.io/zh/messages/common.html

输入图片说明

/mavros/state话题

输入图片说明

通过mavros设置模式其实也是发的base_mode和custom_mode,mavros再将其转为SET_MODE mavlink消息发给飞控
http://docs.ros.org/en/api/mavros_msgs/html/srv/SetMode.html

输入图片说明

base_mode是mavlink里面定义的一些基础的无人机模式信息，是通用的，比如无人机有没有解锁这种基础状态，不管是PX4还是APM或者其他飞控，但凡用mavlink协议，base_mode都是这几个（下面有详细讲），但是custom_mode，mavlink是没有具体给定的，是可以自定义的，不同的飞控定义的custom_mode就可能不同，custom_mode就用于表示自定义的飞行模式，比如px4和apm都有各自的一些定义的飞行模式，不完全相同，具体可以看http://wiki.ros.org/mavros/CustomModes 实际看飞控处于什么飞行模式是看custom_mode的值，但是看无人机有没有解锁是看base_mode。
注意网上看到一些文章说base_mode是当前模式，custom_mode是目标模式，不是这样的。
下图是APM和PX4中各自定义的一些custom_mode
http://wiki.ros.org/mavros/CustomModes

输入图片说明

base_mode

base_mode是一个uint8_t类型，里面有8个Bit位，每个bit位对应的含义如下，在MAV_MODE_FLAG里面有定义。
http://mavlink.io/zh/messages/common.html

输入图片说明

在 MAVLink 中，base_mode 中的每一项的含义如下：

MAV_MODE_FLAG_SAFETY_ARMED (0x80): 表示飞行器的安全模式已设为已解锁（armed）。这意味着电机已启用/运行/可以启动，准备好飞行。这个标志在发送命令 MAV_CMD_DO_SET_MODE 时应被忽略，而应改用 MAV_CMD_COMPONENT_ARM_DISARM。但是这个标志仍然可以用于报告解锁状态。

MAV_MODE_FLAG_MANUAL_INPUT_ENABLED (0x40): 表示遥控器输入已启用。这意味着遥控器的信号可以影响飞行器的控制。

MAV_MODE_FLAG_HIL_ENABLED (0x20): 表示硬件在环仿真模式已启用。在这种模式下，所有电机/执行器都被阻止，但内部软件完全可操作。这个模式通常用于地面仿真。

MAV_MODE_FLAG_STABILIZE_ENABLED (0x10): 表示系统已启用姿态稳定模式。在这种模式下，系统通过电子方式稳定其姿态（有时也可能稳定位置），但需要进一步的控制输入来移动。

MAV_MODE_FLAG_GUIDED_ENABLED (0x08): 表示引导模式已启用，系统飞行航点/任务项。在这种模式下，系统根据预设的航点或任务项进行飞行。

MAV_MODE_FLAG_AUTO_ENABLED (0x04): 表示自主模式已启用，系统自行寻找目标位置。引导标志可以设置或不设置，这取决于实际的实现。

MAV_MODE_FLAG_TEST_ENABLED (0x02): 表示系统已启用测试模式。这个标志用于临时系统测试，不应用于稳定的实现。

MAV_MODE_FLAG_CUSTOM_MODE_ENABLED (0x01): 自定义模式启用，就是是否用custom_mode,比如PX4或APM自定义的custom_mode。

这些标志位可以组合使用以表示飞行器当前的操作模式和状态。例如，如果 base_mode 的值为 217（二进制表示为0b11011001），也就是MAV_MODE_FLAG_SAFETY_ARMED，MAV_MODE_FLAG_MANUAL_INPUT_ENABLED，MAV_MODE_FLAG_STABILIZE_ENABLED，MAV_MODE_FLAG_GUIDED_ENABLED，MAV_MODE_FLAG_CUSTOM_MODE_ENABLED这几个bit位置为1了，表示飞行器已解锁、遥控器输入已启用、姿态稳定模式已启用、引导模式已启用，custom_mode也已经启用。

mavros里面定义的一些base_mode的取值，但是base_mode的取值不仅仅是下面几种，清楚上面含义之后是可以自己进行组合的。
http://docs.ros.org/en/api/mavros_msgs/html/srv/SetMode.html

# basic modes from MAV_MODE
uint8 MAV_MODE_PREFLIGHT = 0
uint8 MAV_MODE_STABILIZE_DISARMED = 80
uint8 MAV_MODE_STABILIZE_ARMED = 208
uint8 MAV_MODE_MANUAL_DISARMED = 64
uint8 MAV_MODE_MANUAL_ARMED = 192
uint8 MAV_MODE_GUIDED_DISARMED = 88
uint8 MAV_MODE_GUIDED_ARMED = 216
uint8 MAV_MODE_AUTO_DISARMED = 92
uint8 MAV_MODE_AUTO_ARMED = 220
uint8 MAV_MODE_TEST_DISARMED = 66
uint8 MAV_MODE_TEST_ARMED = 194

在mavros的代码里可以看到base_mode这么被赋值
mavros/mavros/src/plugins/sys_status.cpp

  base_mode |= (uas->get_armed()) ? enum_value(MAV_MODE_FLAG::SAFETY_ARMED) : 0;
  base_mode |= (uas->get_hil_state()) ? enum_value(MAV_MODE_FLAG::HIL_ENABLED) : 0;
  base_mode |= enum_value(MAV_MODE_FLAG::CUSTOM_MODE_ENABLED);

输入图片说明

custom_mode

这里仅以PX4的custom_mode为例进行讲解，下图是PX4中的custom_mode
http://wiki.ros.org/mavros/CustomModes

输入图片说明

custom_mode是uint32_t类型，逻辑低16位保留，逻辑高16位分别由main_mode和sub_mode组成，其中逻辑最高8位是sub_mode，逻辑次高8位是main_mode，main_mode叫主模式，sub_mode叫子模式，main_mode和sub_mode组合起来得到custom_mode。
基于main_mode和sub_mode得到custom_mode代码如下，参考自https://blog.csdn.net/nieji3057/article/details/127617132

uint32 custom_mode = (main_mode<<16)|(sub_mode<<24);

在px4里面，主模式有"MANUAL""ALTCTL""POSCTL""AUTO""ACRO""OFFBOARD""STABILIZED""RATTITUDE"这8种，其中只有"AUTO"有子模式也就是sub_mode，其他主模式都没有对应子模式，它们的sub_mode都为0，主模式"AUTO"的子模式有"AUTO.READY""AUTO.TAKEOFF""AUTO.LOITER""AUTO.MISSION""AUTO.RTL""AUTO.LAND""AUTO.RTGS""AUTO.FOLLOW_TARGET""AUTO.PRECLAND"这9种。

下面是mavros里定义custom_mode的一些代码
mavros/mavros/src/lib/uas_stringify.cpp

//! PX4 custom mode -> string
static const cmode_map px4_cmode_map{{
  {px4::define_mode(px4::custom_mode::MAIN_MODE_MANUAL), "MANUAL"},
  {px4::define_mode(px4::custom_mode::MAIN_MODE_ACRO), "ACRO"},
  {px4::define_mode(px4::custom_mode::MAIN_MODE_ALTCTL), "ALTCTL"},
  {px4::define_mode(px4::custom_mode::MAIN_MODE_POSCTL), "POSCTL"},
  {px4::define_mode(px4::custom_mode::MAIN_MODE_OFFBOARD), "OFFBOARD"},
  {px4::define_mode(px4::custom_mode::MAIN_MODE_STABILIZED), "STABILIZED"},
  {px4::define_mode(px4::custom_mode::MAIN_MODE_RATTITUDE), "RATTITUDE"},
  {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_MISSION), "AUTO.MISSION"},
  {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_LOITER), "AUTO.LOITER"},
  {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_RTL), "AUTO.RTL"},
  {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_LAND), "AUTO.LAND"},
  {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_RTGS), "AUTO.RTGS"},
  {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_READY), "AUTO.READY"},
  {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_TAKEOFF), "AUTO.TAKEOFF"},
  {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_FOLLOW_TARGET), "AUTO.FOLLOW_TARGET"},
  {px4::define_mode_auto(px4::custom_mode::SUB_MODE_AUTO_PRECLAND), "AUTO.PRECLAND"},
}};

mavros/mavros/include/mavros/px4_custom_mode.h

/**
 * @brief PX4 custom mode
 *
 * This union decodes uint32_t HEARTBEAT.custom_mode
 * and uint32_t SET_MODE.custom_mode.
 */
union custom_mode {
	enum MAIN_MODE : uint8_t {
		MAIN_MODE_MANUAL = 1,
		MAIN_MODE_ALTCTL,
		MAIN_MODE_POSCTL,
		MAIN_MODE_AUTO,
		MAIN_MODE_ACRO,
		MAIN_MODE_OFFBOARD,
		MAIN_MODE_STABILIZED,
		MAIN_MODE_RATTITUDE
	};

	enum SUB_MODE_AUTO : uint8_t {
		SUB_MODE_AUTO_READY = 1,
		SUB_MODE_AUTO_TAKEOFF,
		SUB_MODE_AUTO_LOITER,
		SUB_MODE_AUTO_MISSION,
		SUB_MODE_AUTO_RTL,
		SUB_MODE_AUTO_LAND,
		SUB_MODE_AUTO_RTGS,
		SUB_MODE_AUTO_FOLLOW_TARGET,
		SUB_MODE_AUTO_PRECLAND
	};

	struct {
		uint16_t reserved;
		uint8_t main_mode;
		uint8_t sub_mode;
	};

依据mavros的px4_custom_mode.h里对main_mode和sub_mode的枚举，PX4中每种模式的main_mode和sub_mode整理如下

"ACRO"
      main_mode = 5
      sub_mode = 0

"ALTCTL"
      main_mode = 2
      sub_mode = 0

"POSCTL"
      main_mode = 3
      sub_mode = 0
   
"OFFBOARD"     
      main_mode = 6
      sub_mode = 0
   
"STABILIZED"
      main_mode = 7
      sub_mode = 0
   
"RATTITUDE"
      main_mode = 8
      sub_mode = 0
 
"AUTO.MISSION"
      main_mode = 4
      sub_mode = 4
   
"AUTO.LOITER"
      main_mode = 4
      sub_mode = 3

"AUTO.RTL"
      main_mode = 4
      sub_mode = 5
   
"AUTO.LAND"
      main_mode = 4;
      sub_mode = 6;
   
"AUTO.RTGS"
      main_mode = 4
      sub_mode = 7
   
"AUTO.READY"
      main_mode = 4
      sub_mode = 1
   
"AUTO.TAKEOFF"
      main_mode = 4
      sub_mode = 2

基于具体mavlink消息帧的来看base_mode和custom_mode

这里以mavlink v1的消息帧为例
mavlink v1的消息帧结构如下
https://mavlink.io/en/guide/serialization.html

输入图片说明

QGC切定高时发出的SET_MODE(#11) mavlink消息如下

FE 06 B0 FF BE 0B 00 00 02 00 01 1D B4 AF

切hold时发出的SET_MODE(#11) mavlink消息如下

FE 06 B0 FF BE 0B 00 00 04 03 01 1D E1 B1

切mission时发出的SET_MODE(#11) mavlink消息如下

FE 06 2E FF BE 0B 00 00 04 04 01 1D 43 4C

所以以切hold的消息帧为例，消息帧中payload部分是00 00 04 03 01 1D这六个字节，这六个字节对应了SET_MODE(#11)里面的target_system,base_mode,custom_mode这三项，但是这三项的存储顺序是不一定和Mavlink文档里所列的顺序一样的，具体的存储顺序可以去看此mavlink里对应的消息的头文件里面消息类的定义。
http://mavlink.io/zh/messages/common.html#

输入图片说明

从SET_MODE(#11) mavlink消息对应的头文件mavlink_msg_set_mode.h里可以看到，实际存储顺序是先custom_mode的四个字节，再target_system的1个字节，再base_mode的1个字节，所以payload部分是00 00 04 03 01 1D这六个字节，解析下来，00 00 04 03这四个字节对应custom_mode,01对应target_system,1D对应base_mode。如果按照mavlink文档里对SET_MODE消息里的项列的顺序解析，可能就把04 03 01 1D当作custom_mode了，就会造成错误，这是需要注意的。

输入图片说明

我们可以继续进行分析，custom_mode是00 00 04 03这四个字节，这里需要再说明下mavlink默认是小端模式，基于小端模式来看这四个字节，那么03就是这四个字节的逻辑也是存储地址最高8位，04是这4个字节的逻辑也是存储地址次高8位，所以03是对应sub_mode,04对应main_mode，根据之前讲解的custom_mode,main_mode位3，sub_mode为4时，此时对应的飞行模式就是AUTO.LOITER，也就是HOLD，和QGC切HOLD是对应的。
对于base_mode，十六进制1D对应base_mode，对应的二进制表示是0b00011101，十进制对应29，和前面所讲的MAV_MODE_FLAG表进行对应，0b00011101就对应的MAV_MODE_FLAG_STABILIZE_ENABLED，MAV_MODE_FLAG_GUIDED_ENABLED，MAV_MODE_FLAG_AUTO_ENABLED，MAV_MODE_FLAG_CUSTOM_MODE_ENABLED这几个bit位是置为1的，表示系统姿态稳定模式已启用，引导模式已启用，自主模式已启用，自定义模式启用。

我们也可以结合心跳包进行分析，飞控发出的心跳包我们可以直接在QGC的mavlink inspector里面进行查看，mavlink inspector里面有将心跳包里每项的值解析出来，其所显示的值都是十进制显示的。

输入图片说明

base_mode的值81的二进制是0b01010001，对照MAV_MODE_FLAG表里的定义，其表示遥控器输入已启用，姿态稳定模式已启用，自定义模式已启用。
custom_mode的值为196608，十六进制是 00 03 00 00 ，也就是main_mode是3，sub_mode是0，表示无人机目前处于POSCTL模式也就是定点模式