• PX4模块设计之十四:Event设计


    事件接口将事件uORB消息发布到系统内,其主要作用是通过MAVLink传输层,将消息发送到GCS和其他组件。该接口可用于发布状态变化或任何其他类型事件的事件,包括待命准备、校准完成和达到目标起飞高度等。

    PX4系统里面将这部分内容抽象出一个模块libevents,并在repo上通过submodule的方式引入,体现了设计的模块化。

    [submodule "src/lib/events/libevents"]
            path = src/lib/events/libevents
            url = https://github.com/mavlink/libevents.git
    
    • 1
    • 2
    • 3

    1. Event主要接口

    • event_name: the event name (events::ID(event_name)) 在PX4的整个源代码中必须是唯一的。一般来说,在其前面加上模块名,或较大模块的源文件。
    • Log Level
    Emergency,
    Alert,
    Critical,
    Error,
    Warning,
    Notice,
    Info,
    Debug,
    Disabled,
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • Event Message 单行短字符串。它可能包含参数的模板占位符。
    • Arguments and Enums 事件可以有一组固定的参数,这些参数可以使用模板占位符插入到消息或描述中。

    1.1 字符串消息接口

    platforms\common\include\px4_platform_common\events.h

    inline void send(uint32_t id, const LogLevels &log_levels, const char *message)
    {
    	EventType e{};
    	e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external;
    	e.id = id;
    	CONSOLE_PRINT_EVENT(e.log_level_external, e.id, message);
    	send(e);
    }
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8

    1.2 参数消息接口

    platforms\common\include\px4_platform_common\events.h

    inline void send(uint32_t id, const LogLevels &log_levels, const char *message, Args... args)
    {
    	EventType e{};
    	e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external;
    	e.id = id;
    	static_assert(util::sizeofArguments(args...) <= sizeof(e.arguments), "Too many arguments");
    	util::fillEventArguments((uint8_t *)e.arguments, args...);
    	CONSOLE_PRINT_EVENT(e.log_level_external, e.id, message);
    	send(e);
    }
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10

    1.3 内部uORB实现

    platforms\common\events.cpp,通过ORB_ID(event)消息发不到系统内部,在GCS代码中订阅该消息,并针EventType进行解析。

    void send(EventType &event)
    {
    	event.timestamp = hrt_absolute_time();
    
    	// We need some synchronization here because:
    	// - modifying orb_event_pub
    	// - the update of event_sequence needs to be atomic
    	// - we need to ensure ordering of the sequence numbers: the sequence we set here
    	//   has to be the one published next.
    	pthread_mutex_lock(&publish_event_mutex);
    	event.event_sequence = ++event_sequence; // Set the sequence here so we're able to detect uORB queue overflows
    
    	if (orb_event_pub != nullptr) {
    		orb_publish(ORB_ID(event), orb_event_pub, &event);
    
    	} else {
    		orb_event_pub = orb_advertise_queue(ORB_ID(event), &event, event_s::ORB_QUEUE_LENGTH);
    	}
    
    	pthread_mutex_unlock(&publish_event_mutex);
    }
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    struct EventType {
        uint32_t id;
        uint8_t log_levels;
        uint8_t arguments[40];
    };
    
    • 1
    • 2
    • 3
    • 4
    • 5

    注:关于uORB和MAVLink详见参考资料,这里不再展开。

    2. PX4 Events框架

    事件定义由命名空间分隔。每个名称空间都有一个名称和一个8位组件ID(通常与MAVLink组件ID匹配)。在命名空间中,每个事件都有一个唯一的名称和子ID(24位)。事件ID是具有子ID的组合组件ID。因此,可以通过ID或::。

    rtl_land_at_home在整合libevents到PX4飞控系统时,通过src/lib/events/CMakeLists.txt将PX4和常用的消息定义从代码和配置转换成jason,以便GCS作为配置输入使用。

    总体上,分为两部分:

    1. 通过配置文件汇总,提供GCS或其他应用程序。
    2. 通过代码内嵌(未见明显应用,不过如果该消息是飞控内部使用,可以使用该方式)

    2.1 配置文件

    build/holybro_kakutef7_default/events/all_events.json
     ├──> build/holybro_kakutef7_default/events/px4.json
     │   └──> ${all_px4_src_files}  //PX4工程源代码
     └──> build/holybro_kakutef7_default/events/common_with_enums.json
         ├──> src/lib/events/enums.json 
         └──> src/lib/events/libevents/events/libevents/events/common.json
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6

    2.2 内嵌代码

    build/holybro_kakutef7_default/events/events_generated.h
     └──> build/holybro_kakutef7_default/events/common_with_enums.json
         ├──> src/lib/events/enums.json
         └──> src/lib/events/libevents/events/common.json
    
    • 1
    • 2
    • 3
    • 4

    3. 使用方法

    注:关于代码中消息将会在CMakeList.txt编译之前转换成jason或者.h文件。(编译工具提供的自动化功能)

    3.1 Step 1:包含消息接口头文件

    #include 
    
    • 1

    3.2 Step 2:使用合适的API接口

    字符串消息接口

    events::send(events::ID("navigator_mis_multi_land"), {events::Log::Error, events::LogInternal::Info},
    					     "Mission rejected: more than one land start commands");
    
    • 1
    • 2

    或者 参数消息接口

    events::send(events::ID("navigator_mis_land_approach"), {events::Log::Error, events::LogInternal::Info},
    					    "Mission rejected: adjust landing approach",
    					    (int)ceilf(slope_alt_req - missionitem_previous.altitude),
    					    (int)ceilf(wp_distance_req - wp_distance));
    
    • 1
    • 2
    • 3
    • 4

    3.3 注意事项

    1. 有效参数类型: uint8_t, int8_t, uint16_t, int16_t, uint32_t, int32_t, uint64_t, int64_t and float.
    2. 枚举类型定义
    1. 参数插入事件消息字符串:{n}
    2. 事件描述并非必须,如下例子所述
    /* EVENT
     * @description
     * Land immediately and check the system.
     */
    events::send(
    	events::ID("sensor_failover_baro"), events::Log::Emergency, "Baro sensor #{1} failure: {2}", failover_index,
    	failover_reason);
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    1. 参数表达方式:{ARG_IDX[:.NUM_DECIMAL_DIGITS][UNIT]}
        m: horizontal distance in meters
        m_v: vertical distance in meters
        m^2: area in m^2
        m/s: speed in m/s
        C: temperature in degrees celsius
        NUM_DECIMAL_DIGITS only makes sense for real number arguments
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6

    4. 参考资料

    【1】PX4开源软件框架简明简介
    【2】PX4 Events Interface
    【3】PX4模块设计之二:uORB消息代理
    【4】PX4模块设计之三:自定义uORB消息
    【5】PX4模块设计之四:MAVLink简介
    【6】PX4模块设计之五:自定义MAVLink消息

  • 相关阅读:
    蓝桥杯练习题——多路并归
    SkeyeGisMap地图扩展(四) 添加动态目标
    centos7服务器升级ssh版本&&修复ssh漏洞 CVE-2016-20012、 CVE-2021-41617
    前端网络安全面试题:CSRF 与 XSS
    AMD、request.js,生词太多,傻傻搞不清
    C++二分查找算法:132 模式解法三枚举1
    淘宝平台API接口,爬虫数据(数据参数汇总)
    kotlin
    Redis高可用方案之哨兵模式
    Go内存管理一文足矣
  • 原文地址:https://blog.csdn.net/lida2003/article/details/126048332