zoukankan      html  css  js  c++  java
  • [pixhawk笔记]6-uORB流程及关键函数解析

    本文中将结合代码、文档及注释,给出uORB执行流程及关键函数的解析,由于uORB的机制实现较为复杂,所以本文主要学习如何使用uORB的接口来实现通信。回到上一篇笔记中的代码:

    #include <px4_config.h>
    #include <px4_tasks.h>
    #include <px4_posix.h>
    #include <unistd.h>
    #include <stdio.h>
    #include <poll.h>
    #include <string.h>
    
    #include <uORB/uORB.h>
    #include <uORB/topics/sensor_combined.h>
    #include <uORB/topics/vehicle_attitude.h>
    
    __EXPORT int px4_simple_app_main(int argc, char *argv[]);
    
    int px4_simple_app_main(int argc, char *argv[])
    {
        PX4_INFO("Hello Sky!");
        int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
            orb_set_interval(sensor_sub_fd,200);/* limit the update rate to 5 Hz */
    
        struct vehicle_attitude_s att;
        memset(&att,0,sizeof(att));
        orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude),&att);
    
        px4_pollfd_struct_t fds[] = {
            {.fd = sensor_sub_fd, .events = POLLIN},
        };        
    
        int error_counter = 0;
    
        for(int i=0;i<5;i++){
            int poll_ret = px4_poll(fds,1,1000);
    
            if(poll_ret == 0){
                PX4_ERR("Got no data within a second");
            }else if(poll_ret<0){
                if(error_counter<10 || error_counter % 50 == 0){
                    PX4_ERR("ERROR return value from poll():%d",poll_ret);
                }
    
                error_counter++;
            }else{
                if(fds[0].revents & POLLIN){
                    struct sensor_combined_s raw;
                    orb_copy(ORB_ID(sensor_combined),sensor_sub_fd,&raw);
                    PX4_INFO("Accelerometer:	%8.4f	%8.4f	%8.4f",
                        (double)raw.accelerometer_m_s2[0],
                        (double)raw.accelerometer_m_s2[1],
                        (double)raw.accelerometer_m_s2[2]);
    
                    att.rollspeed = raw.accelerometer_m_s2[0];
                    att.pitchspeed = raw.accelerometer_m_s2[1];
                    att.yawspeed = raw.accelerometer_m_s2[2];
    
                    orb_publish(ORB_ID(vehicle_attitude),att_pub,&att);
                }
            }
        }
        PX4_INFO("exiting");
        return OK;
    }
    
    • 订阅
      可以看出,订阅一个并获取一个主题的信息主要流程及其中关键函数如下:
    • #include <uORB/topics/sensor_combined.h>
      ..
      int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));

      该语句先包含一个主题头文件,该文件内容如下:

    /****************************************************************************
     *
     *   Copyright (C) 2013-2016 PX4 Development Team. All rights reserved.
     *
     * Redistribution and use in source and binary forms, with or without
     * modification, are permitted provided that the following conditions
     * are met:
     *
     * 1. Redistributions of source code must retain the above copyright
     *    notice, this list of conditions and the following disclaimer.
     * 2. Redistributions in binary form must reproduce the above copyright
     *    notice, this list of conditions and the following disclaimer in
     *    the documentation and/or other materials provided with the
     *    distribution.
     * 3. Neither the name PX4 nor the names of its contributors may be
     *    used to endorse or promote products derived from this software
     *    without specific prior written permission.
     *
     * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
     * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
     * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
     * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
     * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
     * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
     * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
     * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
     * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
     * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
     * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
     * POSSIBILITY OF SUCH DAMAGE.
     *
     ****************************************************************************/
    
    /* Auto-generated by genmsg_cpp from file /home/spy/src/Firmware/msg/sensor_combined.msg */
    
    
    #pragma once
    
    #include <stdint.h>
    #ifdef __cplusplus
    #include <cstring>
    #else
    #include <string.h>
    #endif
    
    #include <uORB/uORB.h>
    
    
    #ifndef __cplusplus
    #define RELATIVE_TIMESTAMP_INVALID 2147483647
    
    #endif
    
    
    #ifdef __cplusplus
    struct __EXPORT sensor_combined_s {
    #else
    struct sensor_combined_s {
    #endif
        uint64_t timestamp; // required for logger
        float gyro_rad[3];
        uint32_t gyro_integral_dt;
        int32_t accelerometer_timestamp_relative;
        float accelerometer_m_s2[3];
        uint32_t accelerometer_integral_dt;
        int32_t magnetometer_timestamp_relative;
        float magnetometer_ga[3];
        int32_t baro_timestamp_relative;
        float baro_alt_meter;
        float baro_temp_celcius;
    
    #ifdef __cplusplus
        static constexpr int32_t RELATIVE_TIMESTAMP_INVALID = 2147483647;
    
    #endif
    };
    
    /* register this as object request broker structure */
    ORB_DECLARE(sensor_combined);
    

    可以看出,该文件定义了一个sensor_combined的结构体,该结构体中包含了陀螺仪、加速度计、磁罗盘和气压计的数据。并使用ORB_DECLARE()宏声明一个sensor_combined的主题。该文件由genmsg.cpp由/msg/sensor_combined.msg中的内容生成,该msg文件内容如下:

    #
    # Sensor readings in SI-unit form.
    #
    # These fields are scaled and offset-compensated where possible and do not
    # change with board revisions and sensor updates.
    #
    
    int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
    
    
    # gyro timstamp is equal to the timestamp of the message
    float32[3] gyro_rad			# average angular rate measured in the XYZ body frame in rad/s over the last gyro sampling period
    uint32 gyro_integral_dt		# gyro measurement sampling period in us
    
    int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
    float32[3] accelerometer_m_s2		# average value acceleration measured in the XYZ body frame in m/s/s over the last accelerometer sampling period
    uint32 accelerometer_integral_dt	# accelerometer measurement sampling period in us
    
    int32 magnetometer_timestamp_relative	# timestamp + magnetometer_timestamp_relative = Magnetometer timestamp
    float32[3] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
    
    int32 baro_timestamp_relative		# timestamp + baro_timestamp_relative = Barometer timestamp
    float32 baro_alt_meter			# Altitude, already temp. comp.
    float32 baro_temp_celcius		# Temperature in degrees celsius
    

    可以看出,在.msg文件中主要定义了一些成员变量项,自动生成的.h文件使用这些成员变量生成了结构体, 并使用ORB_DECLARE()宏声明了这些主题,ORB_DECLARE()代码如下:读者也可以按该方法加入自定义的.msg文件,并编写对应的CMakeLists.txt,来生成消息,通过uORB来交换数据。

    /**
     * Declare (prototype) the uORB metadata for a topic (used by code generators).
     *
     * @param _name		The name of the topic.
     */
    #if defined(__cplusplus)
    # define ORB_DECLARE(_name)		extern "C" const struct orb_metadata __orb_##_name __EXPORT
    #else
    # define ORB_DECLARE(_name)		extern const struct orb_metadata __orb_##_name __EXPORT
    #endif
    

    可以看出,ORB_DECLARE()宏实际上定义了一些orb_metadata 的结构体变量,变量名为__orb_prototype_name。
    订阅时指定了ORB_ID,ORB_ID宏如下:

    /**
     * Generates a pointer to the uORB metadata structure for
     * a given topic.
     *
     * The topic must have been declared previously in scope
     * with ORB_DECLARE().
     *
     * @param _name		The name of the topic.
     */
    #define ORB_ID(_name)		&__orb_##_name
    

         其实是取了ORB_DECLARE()宏生成__orb_prototype_name结构体的地址。orb_subscribe的原型如下:

    	/**
    	 * Subscribe to a topic.
    	 *
    	 * The returned value is a file descriptor that can be passed to poll()
    	 * in order to wait for updates to a topic, as well as topic_read,
    	 * orb_check and orb_stat.
    	 *
    	 * Subscription will succeed even if the topic has not been advertised;
    	 * in this case the topic will have a timestamp of zero, it will never
    	 * signal a poll() event, checking will always return false and it cannot
    	 * be copied. When the topic is subsequently advertised, poll, check,
    	 * stat and copy calls will react to the initial publication that is
    	 * performed as part of the advertisement.
    	 *
    	 * Subscription will fail if the topic is not known to the system, i.e.
    	 * there is nothing in the system that has declared the topic and thus it
    	 * can never be published.
    	 *
    	 * Internally this will call orb_subscribe_multi with instance 0.
    	 *
    	 * @param meta    The uORB metadata (usually from the ORB_ID() macro)
    	 *      for the topic.
    	 * @return    ERROR on error, otherwise returns a handle
    	 *      that can be used to read and update the topic.
    	 */
    	int  orb_subscribe(const struct orb_metadata *meta) ;
    

    可看出,其接受参数就是一个orb_metadata的常值指针,从注释看,其返回一个int类型的fd,表示文件描述符,该描述符作为一个句柄,可以用来读取并更新数据,如果订阅失败,则返回ERROR。
    还有个orb_subscribe_multi函数可以用于订阅主题有多个实例的情况:

    	/**
    	 * Subscribe to a multi-instance of a topic.
    	 *
    	 * The returned value is a file descriptor that can be passed to poll()
    	 * in order to wait for updates to a topic, as well as topic_read,
    	 * orb_check and orb_stat.
    	 *
    	 * Subscription will succeed even if the topic has not been advertised;
    	 * in this case the topic will have a timestamp of zero, it will never
    	 * signal a poll() event, checking will always return false and it cannot
    	 * be copied. When the topic is subsequently advertised, poll, check,
    	 * stat and copy calls will react to the initial publication that is
    	 * performed as part of the advertisement.
    	 *
    	 * Subscription will fail if the topic is not known to the system, i.e.
    	 * there is nothing in the system that has declared the topic and thus it
    	 * can never be published.
    	 *
    	 * If a publisher publishes multiple instances the subscriber should
    	 * subscribe to each instance with orb_subscribe_multi
    	 * (@see orb_advertise_multi()).
    	 *
    	 * @param meta    The uORB metadata (usually from the ORB_ID() macro)
    	 *      for the topic.
    	 * @param instance  The instance of the topic. Instance 0 matches the
    	 *      topic of the orb_subscribe() call, higher indices
    	 *      are for topics created with orb_advertise_multi().
    	 * @return    ERROR on error, otherwise returns a handle
    	 *      that can be used to read and update the topic.
    	 *      If the topic in question is not known (due to an
    	 *      ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
    	 *      this function will return -1 and set errno to ENOENT.
    	 */
    	int  orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) ;
    

        该函数可以通过instance参数来指定实例索引。

    订阅了之后可以获取该主题的数据,获取主题数据有如下几个函数可用:

        • px4_poll

    本文开头的代码中即用的该函数,该函数其实是调用的nuttx系统的poll函数,该函数原型如下:

    
    
    /****************************************************************************
     * Name: poll
     *
     * Description:
     *   poll() waits for one of a set of file descriptors to become ready to
     *   perform I/O.  If none of the events requested (and no error) has
     *   occurred for any of  the  file  descriptors,  then  poll() blocks until
     *   one of the events occurs.
     *
     * Inputs:
     *   fds  - List of structures describing file descriptors to be monitored
     *   nfds - The number of entries in the list
     *   timeout - Specifies an upper limit on the time for which poll() will
     *     block in milliseconds.  A negative value of timeout means an infinite
     *     timeout.
     *
     * Return:
     *   On success, the number of structures that have non-zero revents fields.
     *   A value of 0 indicates that the call timed out and no file descriptors
     *   were ready.  On error, -1 is returned, and errno is set appropriately:
     *
     *   EBADF  - An invalid file descriptor was given in one of the sets.
     *   EFAULT - The fds address is invalid
     *   EINTR  - A signal occurred before any requested event.
     *   EINVAL - The nfds value exceeds a system limit.
     *   ENOMEM - There was no space to allocate internal data structures.
     *   ENOSYS - One or more of the drivers supporting the file descriptor
     *     does not support the poll method.
     *
     ****************************************************************************/
    
    int poll(FAR struct pollfd *fds, nfds_t nfds, int timeout)
    

    可以看出,该函数用于阻塞等待文件描述符更新,等待时间为timeout毫秒。该函数返回值大于0表示等到数据更新,0表示没有等到数据更新,小于0则表示发生错误。(返回值的详细情况参见注释)。

        • orb_check
          该函数原型及注释如下:
          	/**
          	 * Check whether a topic has been published to since the last orb_copy.
          	 *
          	 * This check can be used to determine whether to copy the topic when
          	 * not using poll(), or to avoid the overhead of calling poll() when the
          	 * topic is likely to have updated.
          	 *
          	 * Updates are tracked on a per-handle basis; this call will continue to
          	 * return true until orb_copy is called using the same handle. This interface
          	 * should be preferred over calling orb_stat due to the race window between
          	 * stat and copy that can lead to missed updates.
          	 *
          	 * @param handle  A handle returned from orb_subscribe.
          	 * @param updated Set to true if the topic has been updated since the
          	 *      last time it was copied using this handle.
          	 * @return    OK if the check was successful, ERROR otherwise with
          	 *      errno set accordingly.
          	 */
          	int  orb_check(int handle, bool *updated) ;
          

          该函数也是用于检查主题是否有更新,与px4_poll不同的是没有阻塞等待,如果从上次orb_copy函数执行之后,主题发生了更新,则将第二个参数设置为true,否则为false。若函数正确执行,返回OK,否则返回ERROR。相比于px4_poll,该函数不会阻塞等待,所以不会造成系统过载。

        • orb_stat

          	/**
          	 * Return the last time that the topic was updated. If a queue is used, it returns
          	 * the timestamp of the latest element in the queue.
          	 *
          	 * @param handle  A handle returned from orb_subscribe.
          	 * @param time    Returns the absolute time that the topic was updated, or zero if it has
          	 *      never been updated. Time is measured in microseconds.
          	 * @return    OK on success, ERROR otherwise with errno set accordingly.
          	 */
          	int  orb_stat(int handle, uint64_t *time) ;
          

          该函数用于获得指定的主题上一次更新的绝对时间,为毫秒数。返回值为OK表示成功执行,ERROR为有错误发生,具体错误可以查看errno。从orb_check的注释中可以看出,该函数有缺陷,因为在orb_stat和orb_copy之间的时间窗口有可能发生更新,而根据返回的毫秒数来确定可能会错过该更新。

             使用以上三个函数可以确定主题是否发生更新,如有更新发生,则可以使用orb_copy来从主题中获得更新数据:

        • orb_copy
          该函数原型如下:
          	/**
          	 * Fetch data from a topic.
          	 *
          	 * This is the only operation that will reset the internal marker that
          	 * indicates that a topic has been updated for a subscriber. Once poll
          	 * or check return indicating that an updaet is available, this call
          	 * must be used to update the subscription.
          	 *
          	 * @param meta    The uORB metadata (usually from the ORB_ID() macro)
          	 *      for the topic.
          	 * @param handle  A handle returned from orb_subscribe.
          	 * @param buffer  Pointer to the buffer receiving the data, or NULL
          	 *      if the caller wants to clear the updated flag without
          	 *      using the data.
          	 * @return    OK on success, ERROR otherwise with errno set accordingly.
          	 */
          	int  orb_copy(const struct orb_metadata *meta, int handle, void *buffer) ;
          

          代码中调用时形式如下:

           struct sensor_combined_s raw;
           orb_copy(ORB_ID(sensor_combined),sensor_sub_fd,&raw);
          

          第一个参数为ORB_ID获取的主题ID(orb_metadata的指针),第二个参数为文件描述符,可用订阅时返回值,第三个参数为更新数据的缓存地址,可以为一个主题对应结构体类型的地址。
          此外,在订阅时还有其他函数:

        • orb_set_interval(sensor_sub_fd,200);该函数用于设置主题更新速率。

    • 发布
      发布主题主要使用两个函数:
        • orb_advertise(ORB_ID(vehicle_attitude),&att);
          该函数原型及注释如下:
          	// ==== uORB interface methods ====
          	/**
          	 * Advertise as the publisher of a topic.
          	 *
          	 * This performs the initial advertisement of a topic; it creates the topic
          	 * node in /obj if required and publishes the initial data.
          	 *
          	 * Any number of advertisers may publish to a topic; publications are atomic
          	 * but co-ordination between publishers is not provided by the ORB.
          	 *
          	 * Internally this will call orb_advertise_multi with an instance of 0 and
          	 * default priority.
          	 *
          	 * @param meta    The uORB metadata (usually from the ORB_ID() macro)
          	 *      for the topic.
          	 * @param data    A pointer to the initial data to be published.
          	 *      For topics updated by interrupt handlers, the advertisement
          	 *      must be performed from non-interrupt context.
          	 * @param queue_size  Maximum number of buffered elements. If this is 1, no queuing is
          	 *      used.
          	 * @return    nullptr on error, otherwise returns an object pointer
          	 *      that can be used to publish to the topic.
          	 *      If the topic in question is not known (due to an
          	 *      ORB_DEFINE with no corresponding ORB_DECLARE)
          	 *      this function will return nullptr and set errno to ENOENT.
          	 */
          	orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1)
          	{
          		return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT, queue_size);
          	}
          

          该函数用来广播一个主题发布者,返回一个对象指针,该对象可用来发布主题,若出现错误,则返回空指针。
          该函数接收参数为有三个:第一个ORB_ID,表示要发布的主题ID;第二个为结构体指针,指向要发布的数据结构体;第三个为队列大小,默认为1,表示没有消息队列。
          通过查看代码,发现其实该函数调用了orb_advertise_multi函数,使用空指针0作为主题的实例索引,表示只广播主题的第一个实例,并使用默认优先级来广播。
          也可以使用orb_advertise_multi函数。其原型和注释如下:

          /**
          	 * Advertise as the publisher of a topic.
          	 *
          	 * This performs the initial advertisement of a topic; it creates the topic
          	 * node in /obj if required and publishes the initial data.
          	 *
          	 * Any number of advertisers may publish to a topic; publications are atomic
          	 * but co-ordination between publishers is not provided by the ORB.
          	 *
          	 * The multi can be used to create multiple independent instances of the same topic
          	 * (each instance has its own buffer).
          	 * This is useful for multiple publishers who publish the same topic. The subscriber
          	 * then subscribes to all instances and chooses which source he wants to use.
          	 *
          	 * @param meta    The uORB metadata (usually from the ORB_ID() macro)
          	 *      for the topic.
          	 * @param data    A pointer to the initial data to be published.
          	 *      For topics updated by interrupt handlers, the advertisement
          	 *      must be performed from non-interrupt context.
          	 * @param instance  Pointer to an integer which will yield the instance ID (0-based)
          	 *      of the publication. This is an output parameter and will be set to the newly
          	 *      created instance, ie. 0 for the first advertiser, 1 for the next and so on.
          	 * @param priority  The priority of the instance. If a subscriber subscribes multiple
          	 *      instances, the priority allows the subscriber to prioritize the best
          	 *      data source as long as its available. The subscriber is responsible to check
          	 *      and handle different priorities (@see orb_priority()).
          	 * @param queue_size  Maximum number of buffered elements. If this is 1, no queuing is
          	 *      used.
          	 * @return    ERROR on error, otherwise returns a handle
          	 *      that can be used to publish to the topic.
          	 *      If the topic in question is not known (due to an
          	 *      ORB_DEFINE with no corresponding ORB_DECLARE)
          	 *      this function will return -1 and set errno to ENOENT.
          	 */
          	orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
          					 int priority, unsigned int queue_size = 1) ;
          

          该函数可以指定int*类型的instance参数,可以在一个主题存在多个实例时使用。

        • orb_publish(ORB_ID(vehicle_attitude),att_pub,&att);
          该函数用于在广播主题之后发布主题,使用广播时返回的主题句柄。函数原型及注释如下:

          	/**
          	 * Publish new data to a topic.
          	 *
          	 * The data is atomically published to the topic and any waiting subscribers
          	 * will be notified.  Subscribers that are not waiting can check the topic
          	 * for updates using orb_check and/or orb_stat.
          	 *
          	 * @param meta    The uORB metadata (usually from the ORB_ID() macro)
          	 *      for the topic.
          	 * @handle    The handle returned from orb_advertise.
          	 * @param data    A pointer to the data to be published.
          	 * @return    OK on success, ERROR otherwise with errno set accordingly.
          	 */
          	int  orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) ;
          

          该函数用于发布主题,发布之后会通知所有订阅者主题已更新(例如使用px4_poll进行阻塞等待的订阅者),而orb_check需要手动检查是否已更新。

           

  • 相关阅读:
    Longest Palindromic Substring
    PayPal MLSE job description
    Continuous Median
    Remove Duplicates From Linked List
    Valid IP Address
    Longest substring without duplication
    Largest range
    Subarray sort
    Multi String Search
    Suffix Trie Construction
  • 原文地址:https://www.cnblogs.com/spyplus/p/pixhawk_note_uORB_functions.html
Copyright © 2011-2022 走看看