PX4通过I2C方式添加自定义传感器（3）
程序员文章站
2023-12-29 10:19:28
...
添加自定义传感器并实现数据的发送和订阅
1、前期准备
1.1 建立文件夹和相关文件配置
我是在src/drivers/distance_sensor文件夹下操作的，当然其他文件夹下都类似。首先建立了两个文件夹angle_source和angle_detection，前面一个主要是基类的定义，里面有一些函数的申明，后面一个主要是存放主要的程序和实现消息的发布。angle_source这个文件夹我先不说，因为这个文件夹比较简单我是直接把src/drivers/airspeed文件夹下的内容拷贝到我的文件夹下的，然后对它进行一些名字上的修改。我们还是来看angle_detection文件夹吧。可以看到我们建了两个文件，一个.cpp文件一个Cmakelists文件。.cpp文件就是存放代码的，cmakelists文件就是跟编译有关，有了这个文件我们才能让自己写的代码能够参与编译。
1.2 Cmakelists文件编写
我们先看一下代码
px4_add_module(
MODULE angle_detection
MAIN angle_sensor
COMPILE_FLAGS
-Wno-sign-compare
SRCS
angle_sensor.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
关于这个文件的编写，随便找一个Cmakelists文件模仿一下就好了。需要说一下，MAIN就是主函数的名字，SRCS对应的是你.cpp文件的名字，比如我的就是angle_sensor.cpp。
1.3 编译文件nuttx_px4fmu-v2_default.cmake的修改
这个文件就是整个固件的编译文件了，我们只需要在其中加一句就行，这样就和我们刚刚写的Cmakelists文件对接上了，我们的程序就可以参与整体的编译了。
红线所划之处就是我添加的模块。这样我们前期的准备工作就结束了。下面就是具体的编程了。
2、基类的编写
2.1 基类.cpp文件编写
这个其实就是我前面所说的angle_source文件夹下内容的编写，我也说了就是直接把src/drivers/airspeed文件夹下的内容拷贝到我的文件夹下的，然后对它进行一些名字上的修改。
这里我直接就把我的这块程序复制过来了，就不做过多介绍了。
#include <px4_config.h>
#include <drivers/device/device.h>
#include <drivers/device/i2c.h>
#include <systemlib/airspeed.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/angle_sensor.h>
#include <uORB/topics/subsystem_info.h>
#include <drivers/distance_sensor/angle_source/angle.h>
Angle::Angle(int bus, int address, unsigned conversion_interval, const char *path) :
I2C("Angle", path, bus, address, 100000),
_sensor_ok(false),
_last_published_sensor_ok(true), /* initialize differently to force publication */
_measure_ticks(0),
_collect_phase(false),
_diff_pres_offset(0.0f),
_angle_pub(nullptr),
_angle_orb_class_instance(-1),
_subsys_pub(nullptr),
_class_instance(-1),
_conversion_interval(conversion_interval),
_sample_perf(perf_alloc(PC_ELAPSED, "aspd_read")),
_comms_errors(perf_alloc(PC_COUNT, "aspd_com_err"))
{
// enable debug() calls
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
Angle::~Angle()
{
/* make sure we are truly inactive */
stop();
if (_class_instance != -1) {
unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
}
orb_unadvertise(_angle_pub);
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
}
int
Angle::init()
{
/* do I2C init (and probe) first */
if (I2C::init() != PX4_OK) {
return PX4_ERROR;
}
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
measure();
angle_sensor_s data = {};
/* measurement will have generated a report, publish */
_angle_pub = orb_advertise_multi(ORB_ID(angle_sensor), &data,&_angle_orb_class_instance,
ORB_PRIO_HIGH-_class_instance);
if (_angle_pub == nullptr) {
PX4_WARN("uORB started?");
}
return PX4_OK;
}
int
Angle::probe()
{
/* on initial power up the device may need more than one retry
for detection. Once it is running the number of retries can
be reduced
*/
_retries = 4;
int ret = measure();
// drop back to 2 retries once initialised
_retries = 2;
return ret;
}
int
Angle::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signaling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(_conversion_interval);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(_conversion_interval)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
break;
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
case AIRSPEEDIOCSSCALE: {
struct airspeed_scale *s = (struct airspeed_scale *)arg;
_diff_pres_offset = s->offset_pa;
return OK;
}
case AIRSPEEDIOCGSCALE: {
struct airspeed_scale *s = (struct airspeed_scale *)arg;
s->offset_pa = _diff_pres_offset;
s->scale = 1.0f;
return OK;
}
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
void
Angle::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&Angle::cycle_trampoline, this, 1);
}
void
Angle::stop()
{
work_cancel(HPWORK, &_work);
}
void
Angle::update_status()
{
if (_sensor_ok != _last_published_sensor_ok) {
/* notify about state change */
struct subsystem_info_s info = {};
info.present = true;
info.enabled = true;
info.ok = _sensor_ok;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE;
if (_subsys_pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
} else {
_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
_last_published_sensor_ok = _sensor_ok;
}
}
void
Angle::cycle_trampoline(void *arg)
{
Angle *dev = (Angle *)arg;
dev->cycle();
dev->update_status();
}
可以看到我就是把类的名字进行了修改。这个类有什么作用呢？我们不妨再简单看一下这段程序，其实也就是几个函数的定义而已，比如我们看到有初始化函数Angle::init()，还有启动函数start()，还有一个名字是ioctl()，这个函数就是对文件的操作，比如到时候设置采集的频率等等。
2.2 基类Cmakelists文件修改
这个修改也比较简单了，就是几个名字的修改，不做过多介绍，代码如下：
px4_add_module(
MODULE drivers__angle_source
COMPILE_FLAGS
SRCS
angle.cpp
DEPENDS
platforms__common
)
2.3 基类.h文件编写
这个文件就是头文件了，对类和基本函数的申明咯，到时候其他文件只要包含了这个头文件，相应的函数和类就可以使用了，而不必重复声明。代码如下：
#pragma once
#include <string.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_workqueue.h>
#include <systemlib/airspeed.h>
#include <systemlib/perf_counter.h>
#include <uORB/uORB.h>
#include <uORB/topics/angle_sensor.h>
#include <uORB/topics/subsystem_info.h>
/* Default I2C bus */
static constexpr uint8_t PX4_I2C_BUS_DEFAULT = PX4_I2C_BUS_EXPANSION;
class __EXPORT Angle : public device::I2C
{
public:
Angle(int bus, int address, unsigned conversion_interval, const char *path);
virtual ~Angle();
virtual int init();
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
private:
/* this class has pointer data members and should not be copied */
Angle(const Angle &);
Angle &operator=(const Angle &);
protected:
virtual int probe();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
virtual void cycle() = 0;
virtual int measure() = 0;
virtual int collect() = 0;
/**
* Update the subsystem status
*/
void update_status();
work_s _work;
bool _sensor_ok;
bool _last_published_sensor_ok;
uint32_t _measure_ticks;
bool _collect_phase;
float _diff_pres_offset;
orb_advert_t _angle_pub;
int _angle_orb_class_instance;
orb_advert_t _subsys_pub;
int _class_instance;
unsigned _conversion_interval;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
/**
* add a new report to the reports queue
*
* @param report differential_pressure_s report
*/
void new_report(const angle_sensor_s &volt_data);
};
3、继承类的编写
从这里开始就进入到我们真正需要好好编写的地方了，需要根据自己的需要进行适当的修改。
-首先我们来看一些基本的常量的定义：
/* I2C bus address */
#define I2C_ADDRESS 0x48 /* 7-bit I2C ADDRESS */
#define SET_CMD 0x40|3 /*Choose the channel */
#define Anglesensor_BUS PX4_I2C_BUS_EXPANSION
#define angle_PATH "/dev/angle_detection"第一个定义的是I2C的地址，还是那句话这里的地址是7位的，这里需要根据具体的器件地址进行修改，地址如果不对了，什么数据都读不出来了。第二个定义的是一个控制命令，因为我的器件在读之前需要先进行通道的选择等一系列操作，所以需要先写入一个控制命令，如果你的器件在读之前也需要进行写控制命令也可以在这里进行定义。第三个定义的是I2C的总线有PX4_I2C_BUS_EXPANSION和PX4_I2C_BUS_ONBOARD可以选择，因为我的器件属于板外扩展的，所以就选择这个。第四个是路径的定义，定义了路径之后，一切都可以以文件的形式进行操作了，但是关于文件操作具体如何实现，这个我也不太清楚。
- 接着就是类的声明了
class Anglesensor:public Angle
{
public:
Anglesensor( int bus , int address,const char *path);
virtual int collect();
protected:
virtual int measure();
// virtual int collect();
virtual void cycle();
};
Anglesensor::Anglesensor( int bus , int address=I2C_ADDRESS,const char *path=angle_PATH):
Angle(bus,address,10000,path)
{
_device_id.devid_s.devtype=DRV_DIFF_PRESS_DEVTYPE_MS5525;
}
看到这里的类就是对我们刚刚建立的基类的公有继承了，比较简单。
- 然后就是相关函数的定义
int
Anglesensor::measure()
{
int ret=-EIO;
/*
* Send the command to begin a measurement.
*/
uint8_t cmd=SET_CMD;
ret = transfer(&cmd,1,nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
}
return ret;
}
int
Anglesensor::collect()
{
int ret = -EIO;
uint8_t val;
angle_sensor_s volt_data; /*define a struct to collect data*/
/*read the data*/
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val,1);
if (ret < 0) {
perf_count(_comms_errors);
return ret;
}
PX4_INFO("volt data1:%d",val); //add by klb 5/4
volt_data.error_count=perf_event_count(_comms_errors);
volt_data.volt=val;
volt_data.volt_b=val/50; //transfer to volt,b:the integer before point
volt_data.volt_a=(val%50)/10; //a:the integer after point
volt_data.device_id=0;
PX4_INFO("volt data2:%d",volt_data.volt); //add by klb 5/4
/*to publish a topic */
if (_angle_pub != nullptr && !(_pub_blocked)) {
orb_publish(ORB_ID(angle_sensor), _angle_pub, &volt_data);
}
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
Anglesensor::cycle()
{
int ret;
if(_collect_phase){
ret=collect();
if(OK!=ret){
perf_count(_comms_errors);
start();
_sensor_ok=false;
return;
}
_collect_phase=false;
if(_measure_ticks>USEC2TICK(10000)){
work_queue(HPWORK,
&_work,
(worker_t)&Angle::cycle_trampoline,
this,
_measure_ticks-USEC2TICK(10000));
return;
}
}
ret=measure();
if(OK!=ret){
DEVICE_DEBUG("measure error");
}
_sensor_ok=(ret==OK);
_collect_phase=true;
/*schedule a fresh cycle call when the measurement is done*/
work_queue(HPWORK,
&_work,
(worker_t)&Angle::cycle_trampoline,
this,
USEC2TICK(10000));
}
这里定义了measure()函数就是在读之前先发送控制命令的，collect()函数就是进行数据的读取，及一些简单的对数据结果的处理。处理完了之后我们看到有对数据的发布：
/*to publish a topic */
if (_angle_pub != nullptr && !(_pub_blocked)) {
orb_publish(ORB_ID(angle_sensor), _angle_pub, &volt_data);
}
这里的angle_sensor是我自己定义的一个topic，关于topic的定义也比较简单相关的文章也很多，你可以之间搜uORB添加自定义主题，就会有相关的文章。定义了主题之后，通过公告、发布、订阅，这样其他的app也可以使用你这个传感器的数据了，我们后面还会有这方面的操作。
关于cycle()这个函数看起来比较复杂，其实就可以理解为是一种循环进行读取。
- 最后就是一些提供给nsh调试的函数
上一篇文章也提到了，我们看一下代码：
/**
* Local function in support of the shell command
*/
namespace anglesensor {
Anglesensor *cal;
int start(int i2c_bus);
int stop();
int test();
int reset();
/**
* @brief start
* @param i2c_bus
* start the driver
*/
int
start(int i2c_bus)
{
int fd;
if(cal!=nullptr){
PX4_ERR("already started");
return PX4_ERROR;
}
/*creat the driver*/
cal=new Anglesensor(i2c_bus);
if (cal==nullptr){
goto fail;
}
if(OK!=cal->init()){
PX4_ERR("i2c not started!");
goto fail;
}
/*set the poll rate to default,starts automatic data collection*/
fd=open(angle_PATH,O_RDONLY);
if (fd<0){
PX4_ERR("failed to open");
goto fail;
}
if(px4_ioctl(fd,SENSORIOCSPOLLRATE,SENSOR_POLLRATE_DEFAULT)<0){
PX4_ERR("failed to set default");
goto fail;
}
return PX4_OK;
fail:
if(cal!=nullptr){
delete cal;
cal=nullptr;
}
PX4_WARN("driver not started on bus %d",i2c_bus);
return PX4_ERROR;
}
/**
* @brief stop
* @stop the driver
*/
int
stop()
{
if(cal!=nullptr){
delete cal;
cal=nullptr;
}else {
PX4_ERR("driver not running");
return PX4_ERROR;
}
return PX4_OK;
}
/**
* @brief test
* perform some basic funcational tests one driver;
* make sure we can collect data from the sensor in polled
* and automatic modes
*/
int
test()
{
// struct angle_sensor_s volt_da;
ssize_t sz;
int ret;
int fd=px4_open(angle_PATH,O_RDONLY);
if(fd<0){
PX4_ERR("%s open failed try'angle_sensor start'if the driver is not running",angle_PATH);
return PX4_ERROR;
}
/*do a simple demmand read*/
struct angle_sensor_s volt_da;
int angle_sub_fd=orb_subscribe(ORB_ID(angle_sensor));
orb_copy(ORB_ID(angle_sensor),angle_sub_fd,&volt_da);
PX4_INFO("volt data3:%d",volt_da.volt);
sz=px4_read(fd,&volt_da,sizeof(volt_da));
if(sz!=sizeof(volt_da)){
PX4_ERR("immediate read failed");
return PX4_ERROR;
}
PX4_INFO("single read");
PX4_INFO("transfer volt data: %d.%d V",volt_da.volt_b,volt_da.volt_a);
/*start the sensor polling at 2HZ*/
if(OK!=px4_ioctl(fd,SENSORIOCSPOLLRATE,2)){
PX4_ERR("failed to set 2 HZ poll rate");
return PX4_ERROR;
}
/*read the sensor 5x and report each value*/
for(unsigned i=0;i<5;i++){
px4_pollfd_struct_t fds;
/*wait for data to be ready*/
fds.fd=fd;
fds.events=POLLIN;
ret=px4_poll(&fds,1,2000);
if(ret!=1){
PX4_ERR("time out waiting for the sensor data");
return PX4_ERROR;
}
/*now to get it*/
sz=px4_read(fd,&volt_da,sizeof(volt_da));
if(sz!=sizeof(volt_da)){
PX4_ERR("periodic read failed");
}
PX4_INFO("periodic read %u",i);
PX4_INFO("volt data: %d.%d",volt_da.volt_b,volt_da.volt_a);
}
/*reset the sensor polling to default rate*/
if(OK!=px4_ioctl(fd,SENSORIOCGPOLLRATE,SENSOR_POLLRATE_DEFAULT)){
PX4_ERR("failed to set default rate");
return PX4_ERROR;
}
return PX4_OK;
}
/**
* @brief reset
* @reset the driver
*/
int reset()
{
int fd=px4_open(angle_PATH,O_RDONLY);
if(fd<0){
PX4_ERR("failed");
return PX4_ERROR;
}
if(px4_ioctl(fd,SENSORIOCSPOLLRATE,0)<0){
PX4_ERR("driver reset failed");
return PX4_ERROR;
}
if(px4_ioctl(fd,SENSORIOCSPOLLRATE,SENSOR_POLLRATE_DEFAULT)<0){
PX4_ERR("driver poll restart failed");
return PX4_ERROR;
}
return PX4_ERROR;
}
}start()函数就是进行一些初始化的设置，另外如果我们需要nsh调试看我们能否得到传感器的数据时，必须首先要进行start()。关于test()函数，其实就是两个功能：
(1). 进行单个数据的读取；
(2). 设置读取频率为2HZ，连续读取五次。
但是在这里我也遇到了问题,
sz=px4_read(fd,&volt_da,sizeof(volt_da));
if(sz!=sizeof(volt_da)){
PX4_ERR("immediate read failed");
return PX4_ERROR;
}
}
就是在这里进行读的时候总是出现错误，显示“immediate read failed”，我琢磨了很久也不知道是什么原因，我觉得可能是路径方面出了问题，但是找相关的资料也找不到。但是还好，虽然这里读不出来，但是主题是成功地发布了，而且可以成功地进行订阅。所以我在这加了一句消息订阅进行检测：
struct angle_sensor_s volt_da;
int angle_sub_fd=orb_subscribe(ORB_ID(angle_sensor));
orb_copy(ORB_ID(angle_sensor),angle_sub_fd,&volt_da);
PX4_INFO("volt data3:%d",volt_da.volt);
既然成功了，我就没管它了。于是下面我们就进行在其他app里进行订阅，看看能否成功。
4、 在px4_simple_app中进行订阅
px4_simple_app在src/examples路径下，这是已经为我们提供好了的一个app例程，我们只需要简单进行修改就好了。还是直接看我修改后的文件吧：
/**
* @file px4_simple_app.c
* Minimal application example for PX4 autopilot
*
* @author Example User <aaa@qq.com>
*/
#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 <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/angle_sensor.h>
__EXPORT int px4_simple_ap_main(int argc, char *argv[]);
int px4_simple_ap_main(int argc, char *argv[])
{
PX4_INFO("Hello KLB!");
/* subscribe to sensor_combined topic */
int sensor_sub_fd = orb_subscribe(ORB_ID(angle_sensor));
/* limit the update rate to 5 Hz */
orb_set_interval(sensor_sub_fd, 200);
/* one could wait for multiple topics with this technique, just using one here */
px4_pollfd_struct_t fds[] = {
{ .fd = sensor_sub_fd, .events = POLLIN },
/* there could be more file descriptors here, in the form like:
* { .fd = other_sub_fd, .events = POLLIN },
*/
};
int error_counter = 0;
for (int i = 0; i < 20; i++) {
/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
int poll_ret = px4_poll(fds, 1, 1000);
/* handle the poll result */
if (poll_ret == 0) {
/* this means none of our providers is giving us data */
PX4_ERR("Got no data within a second");
} else if (poll_ret < 0) {
/* this is seriously bad - should be an emergency */
if (error_counter < 10 || error_counter % 50 == 0) {
/* use a counter to prevent flooding (and slowing us down) */
PX4_ERR("ERROR return value from poll(): %d", poll_ret);
}
error_counter++;
} else {
if (fds[0].revents & POLLIN) {
/* obtained data for the first file descriptor */
struct angle_sensor_s raw;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(angle_sensor), sensor_sub_fd, &raw);
PX4_INFO("num:%d \n\t\t volt:%d.%d V",
raw.volt,
raw.volt_b,
raw.volt_a);
}
/* there could be more file descriptors here, in the form like:
* if (fds[1..n].revents & POLLIN) {}
*/
}
}
PX4_INFO("exiting");
return 0;
}
这里说一下两个地方：
(1). orb_set_interval(sensor_sub_fd, 200); 这个是对接收频率的设置，设置间隔为200ms也就是5HZ
(2).然后我在这里是设置了读取20次
另外需要注意的一点是，px4_simple_app也是需要加入到编译中去的，所以也需要在px4fmu-v2_default当中加入的，具体的方法和上面的一样。
4、加入自启动
为什么要加入自启动呢？这跟我们上面说的一个地方有关，上面我提到，如果我们要获取传感器的数据，我们首先需要先输入angle_sensor start这个命令，也就是启动命令。那么这是在调试中我们可以这么办，那么在实际板子上谁给你输入这个启动命令呢？没有人，那你就只能自启动了。
我们来看看自启动在哪操作，其实也是很简单。我们找到src/Firmware/ROMFS/px4fmu_common/init.d下的rc.sensor这个文件，这里就是对传感器的自启动了。
这里一定要找对，是在PX4FMU_V2下进行添加，红色线标注的部分就是添加的内容，这样他就可以自启动啦。我们直接输入px4_simple_ap就可以直接看到数据啦！
5、nsh调试
我们直接来看看调试的结果啦：
我们看到结果正是我们想要的，这样一切就大功告成了！
ALL GLORY TO GOD !