pixhawk uORB初步分析
再次编辑，因为发现大神的解析，添加在最后，若一般人我不告诉他
由于上节分析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 = -Os2.接收方
首先用_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);
抛砖引玉：一定要戳我，神来之笔