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


抛砖引玉:一定要戳我,神来之笔

你可能感兴趣的:(pixhawk uORB初步分析)