custom_mode 用于表示自定义的飞行模式,比如px4和apm都有各自的一些定义的飞行模式,不完全相同,具体可以看http://wiki.ros.org/mavros/CustomModes。现在也可以明白为什么px4把这些模式叫custom_mode了。所以真正看飞控处于什么飞行模式还是得看custom_mode的值。
注意不是网上看到的一些说base_mode是当前模式,custom_mode是目标模式,不是的!!!
base_mode
base_mode是一个uint8_t类型,里面又8个Bit位,每个bit位对应的含义如下,在MAV_MODE_FLAG里面有定义
所以在mavros的代码里看到base_mode这么被赋值,或者从base_mode里面获取一些比如是否解锁的信息,也就很好理解了。
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 -> 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"},
}};
custom_mode是uint32_t类型,逻辑低16位保留,逻辑高16位分别由main_mode和sub_mode组成
mavros/include/mavros/px4_custom_mode.h · 马熙/mavros - Gitee.com
/**
* @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;
};
https://blog.csdn.net/nieji3057/article/details/127617132
uint32 custom_mode = (main_mode<<16)|(sub_mode<<24);