pixhawk uORB初步分析

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


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



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

// update internal state if new GPS information is available
bool
AP_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 __cplusplus
struct __EXPORT vehicle_gps_position_s {
#else
struct vehicle_gps_position_s {
#endif
uint64_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 _name		The name of the topic.
* @param _struct	The 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	= gps

SRCS		= gps.cpp \
gps_helper.cpp \
mtk.cpp \
ashtech.cpp \
ubx.cpp

MODULE_STACKSIZE = 1200

MAXOPTIMIZATION	 = -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

int
GPS_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中添加

#MODULES	+= drivers/gps

至于以下函数具体细节暂不探讨，知道是这样用

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);

抛砖引玉：一定要戳我，神来之笔