pixhawk uORB初步分析

来源：互联网发布：ubuntu下安装samba 编辑：程序博客网时间：2024/05/18 19:45

再次编辑，因为发现大神的解析，添加在最后，若一般人我不告诉他

根据自己理解画的流程图：（2016.05.29加）

由于上节分析GPS涉及到AP_GPS_PX4::read函数，

// update internal state if new GPS information is availableboolAP_GPS_PX4::read(void){    bool updated = false;    orb_check(_gps_sub, &updated);        if (updated) {        if (OK == orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps_pos)) {            state.last_gps_time_ms = AP_HAL::millis();            state.status  = (AP_GPS::GPS_Status) (_gps_pos.fix_type | AP_GPS::NO_FIX);            state.num_sats = _gps_pos.satellites_used;            state.hdop = uint16_t(_gps_pos.eph*100.0f + .5f);            if (_gps_pos.fix_type >= 2) {                state.location.lat = _gps_pos.lat;                state.location.lng = _gps_pos.lon;                state.location.alt = _gps_pos.alt/10;                state.ground_speed = _gps_pos.vel_m_s;                state.ground_course_cd = wrap_360_cd(degrees(_gps_pos.cog_rad)*100);                state.hdop = _gps_pos.eph*100;                // convert epoch timestamp back to gps epoch - evil hack until we get the genuine                // raw week information (or APM switches to Posix epoch ;-) )                uint64_t epoch_ms = uint64_t(_gps_pos.time_utc_usec/1000.+.5);                uint64_t gps_ms = epoch_ms - DELTA_POSIX_GPS_EPOCH + LEAP_MS_GPS_2014;                state.time_week = uint16_t(gps_ms / uint64_t(MS_PER_WEEK));                state.time_week_ms = uint32_t(gps_ms - uint64_t(state.time_week)*MS_PER_WEEK);                if (_gps_pos.time_utc_usec == 0) {                  // This is a work-around for https://github.com/PX4/Firmware/issues/1474                  // reject position reports with invalid time, as APM adjusts it's clock after the first lock has been aquired                  state.status = AP_GPS::NO_FIX;                }            }            if (_gps_pos.fix_type >= 3) {                state.have_vertical_velocity = _gps_pos.vel_ned_valid;                state.velocity.x = _gps_pos.vel_n_m_s;                state.velocity.y = _gps_pos.vel_e_m_s;                state.velocity.z = _gps_pos.vel_d_m_s;                state.speed_accuracy = _gps_pos.s_variance_m_s;                state.have_speed_accuracy = true;            }            else {                state.have_vertical_velocity = false;            }        }    }    return updated;}

其中包含orb_check(_gps_sub, &updated); if (OK == orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps_pos))。故在此节做一个初步分析。

假如我需要添加一个新的数据源进行进程间的通信，比如GPS,其逻辑应该是这样的：

首先创建文件ardupilot/modules/PX4Firmware/src/modules/uORB/topics/vehicle_gps_position.

里面的内容包含2方面，一个是数据结构，一个是ORB_DECLARE(vehicle_gps_position);

数据结构如下

ardupilot/modules/PX4Firmware/src/modules/uORB/topics/vehicle_gps_position.h#ifdef __cplusplusstruct __EXPORT vehicle_gps_position_s {#elsestruct vehicle_gps_position_s {#endifuint64_t timestamp_position;int32_t lat;int32_t lon;int32_t alt;int32_t alt_ellipsoid;uint64_t timestamp_variance;float s_variance_m_s;float c_variance_rad;uint8_t fix_type;float eph;float epv;float hdop;float vdop;int32_t noise_per_ms;int32_t jamming_indicator;uint64_t timestamp_velocity;float vel_m_s;float vel_n_m_s;float vel_e_m_s;float vel_d_m_s;float cog_rad;bool vel_ned_valid;uint64_t timestamp_time;uint64_t time_utc_usec;uint8_t satellites_used;#ifdef __cplusplus#endif};

然后在ardupilot/modules/PX4Firmware/src/modules/uORB/objects_common.cpp

#include "topics/vehicle_gps_position.h"ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);

/** * Define (instantiate) the uORB metadata for a topic. * * The uORB metadata is used to help ensure that updates and * copies are accessing the right data. * * Note that there must be no more than one instance of this macro * for each topic. * * @param _nameThe name of the topic. * @param _structThe structure the topic provides. */#define ORB_DEFINE(_name, _struct)\const struct orb_metadata __orb_##_name = {\#_name,\sizeof(_struct)\}; struct hack__BEGIN_DECLS

这样就把vehicle_gps_position和结构体vehicle_gps_position_s对应起来了

ORB_DECLARE(vehicle_gps_position);

ardupilot/modules/PX4Firmware/src/modules/uORB/uOrb.h

/** * ORB_DECLARE的宏定义，实际上就是让外界可以使用_name这个所表示的结构体数据 */#if defined(__cplusplus)# define ORB_DECLARE(_name)     extern "C" const structorb_metadata __orb_##_name __EXPORT# defineORB_DECLARE_OPTIONAL(_name)    extern "C"const struct orb_metadata __orb_##_name __EXPORT#else# define ORB_DECLARE(_name)     extern const struct orb_metadata__orb_##_name __EXPORT# defineORB_DECLARE_OPTIONAL(_name)    externconst struct orb_metadata __orb_##_name __EXPORT#endif

接着通过ORB_ID(vehicle_gps_position) 产生一个指针指向结构体vehicle_gps_position_s

#define ORB_ID(_name)&__orb_##_name

至此将数据结构体定义与函数的输入联系起来了
接下来就是分析以下函数的使用过程

ardupilot/libraries/AP_GPS/AP_GPS_PX4.cpp

orb_check(_gps_sub, &updated);

orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps_pos);

_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));

ardupilot/modules/PX4Firmware/src/drives/gps/gps.cpp

orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);

orb_advert_t _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);

1.发送方

首先按以上方式建立好数据结构vehicle_gps_position_svehicle_gps_position_s(用于进程间通讯)

然后通过读取传感器得到具体的数据，存入结构体中

对发布主题进行公告，同时获取公告主题的句柄

orb_advert_t _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);

最后用orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); 结合之前获得的主题ID和句柄以及结构体完成数据发布。

至此，数据发布完毕。为了满足编译的条件，我们要添加ardupilot/modules/PX4Firmware/src/drives/gps/module.mk

MODULE_COMMAND= gpsSRCS= gps.cpp \  gps_helper.cpp \  mtk.cpp \  ashtech.cpp \  ubx.cppMODULE_STACKSIZE = 1200MAXOPTIMIZATION = -Os

2.接收方

首先用_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));进行订阅

有的其它数据需要设置订阅的查询时间间隔（GPS暂时没看到需要设置）比如

ardupilot/modules/PX4Firmware/src/drives/px4fmu/fmu.cpp

for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {if (_control_subs[i] > 0) {orb_set_interval(_control_subs[i], update_rate_in_ms);}}

建立pollfd结构体，用于查询设备状态

/* This is the Nuttx variant of the standard pollfd structure. */struct pollfd{  int         fd;       /* The descriptor being polled */  sem_t      *sem;      /* Pointer to semaphore used to post output event */  pollevent_t events;   /* The input event flags */  pollevent_t revents;  /* The output event flags */  FAR void   *priv;     /* For use by drivers */};

ardupilot/modules/PX4Firmware/src/drives/gps/gps_helper.cpp

intGPS_Helper::poll_or_read(int fd, uint8_t *buf, size_t buf_length, uint64_t timeout){#ifndef __PX4_QURT/* For non QURT, use the usual polling. */pollfd fds[1];//建立pollfd结构体，用于查询设备状态fds[0].fd = fd;//赋值fds[0].events = POLLIN;//赋值/* Poll for new data,  */int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);//阻塞timeout秒，返回值：0表示未跟新数据，<0表示数据跟新错误，其它表示主题状态发生改变if (ret > 0) {/* if we have new data from GPS, go handle it */if (fds[0].revents & POLLIN) {       //判断主题产生了跟新/* * We are here because poll says there is some data, so this * won't block even on a blocking device. But don't read immediately * by 1-2 bytes, wait for some more data to save expensive read() calls. * If more bytes are available, we'll go back to poll() again. */usleep(GPS_WAIT_BEFORE_READ * 1000);return ::read(fd, buf, buf_length);} else {return -1;}} else {return ret;}#else/* For QURT, just use read for now, since this doesn't block, we need to slow it down * just a bit. */usleep(10000);return ::read(fd, buf, buf_length);#endif}

到此为设置订阅的查询时间间隔

接下来是

利用_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));订阅主题，并获取相应的句柄_gps_sub

利用orb_check(_gps_sub, &updated);检查主题是否跟新，其中bool updated = false;

利用orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps_pos);接收跟新的主题

最后在ardupilot/mk/PX4/px4_common.mk中添加