fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 03:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move all drivers to multi pub/sub API

Browse Source
This commit is contained in:
Lorenz Meier
2015-01-29 16:33:53 +01:00
parent cc7a00b96e
commit d851a630d8
8 changed files with 87 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/drv_accel.h
+2 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -81,9 +81,7 @@ struct accel_scale {
/*
* ObjDev tag for raw accelerometer data.
*/
ORB_DECLARE(sensor_accel0);
ORB_DECLARE(sensor_accel1);
ORB_DECLARE(sensor_accel2);
ORB_DECLARE(sensor_accel);
/*
* ioctl() definitions
src/drivers/drv_baro.h
+2 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -63,9 +63,7 @@ struct baro_report {
/*
* ObjDev tag for raw barometer data.
*/
ORB_DECLARE(sensor_baro0);
ORB_DECLARE(sensor_baro1);
ORB_DECLARE(sensor_baro2);
ORB_DECLARE(sensor_baro);
/*
* ioctl() definitions
src/drivers/drv_blinkm.h
+1 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
src/drivers/drv_gyro.h
+2 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -81,9 +81,7 @@ struct gyro_scale {
/*
* ObjDev tag for raw gyro data.
*/
ORB_DECLARE(sensor_gyro0);
ORB_DECLARE(sensor_gyro1);
ORB_DECLARE(sensor_gyro2);
ORB_DECLARE(sensor_gyro);
/*
* ioctl() definitions
src/drivers/l3gd20/l3gd20.cpp
+20 -20
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,7 +33,7 @@
/**
* @file l3gd20.cpp
* Driver for the ST L3GD20 MEMS gyro connected via SPI.
* Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI.
*
* Note: With the exception of the self-test feature, the ST L3G4200D is
* also supported by this driver.
@@ -179,6 +179,12 @@ static const int ERROR = -1;
#define L3GD20_DEFAULT_FILTER_FREQ 30
#define L3GD20_TEMP_OFFSET_CELSIUS 40
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
#endif
@@ -221,7 +227,7 @@ private:
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
orb_id_t _orb_id;
int _orb_class_instance;
int _class_instance;
unsigned _current_rate;
@@ -273,6 +279,13 @@ private:
*/
void disable_i2c();
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@@ -391,7 +404,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_orb_id(nullptr),
_orb_class_instance(-1),
_class_instance(-1),
_current_rate(0),
_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@@ -456,20 +469,6 @@ L3GD20::init()
_class_instance = register_class_devname(GYRO_DEVICE_PATH);
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
_orb_id = ORB_ID(sensor_gyro0);
break;
case CLASS_DEVICE_SECONDARY:
_orb_id = ORB_ID(sensor_gyro1);
break;
case CLASS_DEVICE_TERTIARY:
_orb_id = ORB_ID(sensor_gyro2);
break;
}
reset();
measure();
@@ -478,7 +477,8 @@ L3GD20::init()
struct gyro_report grp;
_reports->get(&grp);
_gyro_topic = orb_advertise(_orb_id, &grp);
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_gyro_topic < 0) {
debug("failed to create sensor_gyro publication");
@@ -1050,7 +1050,7 @@ L3GD20::measure()
/* publish for subscribers */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_orb_id, _gyro_topic, &report);
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
_read++;
src/drivers/lsm303d/lsm303d.cpp
+21 -21
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -211,6 +211,12 @@ static const int ERROR = -1;
#define LSM303D_ONE_G 9.80665f
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
@@ -275,7 +281,7 @@ private:
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_orb_class_instance;
int _accel_class_instance;
unsigned _accel_read;
@@ -329,6 +335,13 @@ private:
*/
void disable_i2c();
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@@ -539,7 +552,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
_mag_range_scale(0.0f),
_mag_samplerate(0),
_accel_topic(-1),
_accel_orb_id(nullptr),
_accel_orb_class_instance(-1),
_accel_class_instance(-1),
_accel_read(0),
_mag_read(0),
@@ -618,7 +631,6 @@ LSM303D::init()
if (_accel_reports == nullptr)
goto out;
/* advertise accel topic */
_mag_reports = new RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr)
@@ -641,7 +653,8 @@ LSM303D::init()
_mag_reports->get(&mrp);
/* measurement will have generated a report, publish */
_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, &_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
if (_mag->_mag_topic < 0) {
warnx("ADVERT ERR");
@@ -654,21 +667,8 @@ LSM303D::init()
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
switch (_accel_class_instance) {
case CLASS_DEVICE_PRIMARY:
_accel_orb_id = ORB_ID(sensor_accel0);
break;
case CLASS_DEVICE_SECONDARY:
_accel_orb_id = ORB_ID(sensor_accel1);
break;
case CLASS_DEVICE_TERTIARY:
_accel_orb_id = ORB_ID(sensor_accel2);
break;
}
_accel_topic = orb_advertise(_accel_orb_id, &arp);
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_accel_topic < 0) {
warnx("ADVERT ERR");
@@ -1556,7 +1556,7 @@ LSM303D::measure()
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_accel_orb_id, _accel_topic, &accel_report);
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
_accel_read++;
src/drivers/mpu6000/mpu6000.cpp
+24 -39
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -175,6 +175,12 @@
#define MPU6000_ONE_G 9.80665f
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
/*
the MPU6000 can only handle high SPI bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
@@ -234,7 +240,7 @@ private:
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_orb_class_instance;
int _accel_class_instance;
RingBuffer *_gyro_reports;
@@ -360,6 +366,13 @@ private:
*/
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
/**
* Measurement self test
*
@@ -457,7 +470,7 @@ protected:
private:
MPU6000 *_parent;
orb_advert_t _gyro_topic;
orb_id_t _gyro_orb_id;
int _gyro_orb_class_instance;
int _gyro_class_instance;
/* do not allow to copy this class due to pointer data members */
@@ -479,7 +492,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_accel_orb_id(nullptr),
_accel_orb_class_instance(-1),
_accel_class_instance(-1),
_gyro_reports(nullptr),
_gyro_scale{},
@@ -613,22 +626,8 @@ MPU6000::init()
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
switch (_accel_class_instance) {
case CLASS_DEVICE_PRIMARY:
_accel_orb_id = ORB_ID(sensor_accel0);
break;
case CLASS_DEVICE_SECONDARY:
_accel_orb_id = ORB_ID(sensor_accel1);
break;
case CLASS_DEVICE_TERTIARY:
_accel_orb_id = ORB_ID(sensor_accel2);
break;
}
_accel_topic = orb_advertise(_accel_orb_id, &arp);
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_accel_topic < 0) {
warnx("ADVERT FAIL");
@@ -639,22 +638,8 @@ MPU6000::init()
struct gyro_report grp;
_gyro_reports->get(&grp);
switch (_gyro->_gyro_class_instance) {
case CLASS_DEVICE_PRIMARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro0);
break;
case CLASS_DEVICE_SECONDARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro1);
break;
case CLASS_DEVICE_TERTIARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro2);
break;
}
_gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_gyro->_gyro_topic < 0) {
warnx("ADVERT FAIL");
@@ -1763,12 +1748,12 @@ MPU6000::measure()
perf_begin(_controller_latency_perf);
perf_begin(_system_latency_perf);
/* publish it */
orb_publish(_accel_orb_id, _accel_topic, &arb);
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
/* stop measuring */
@@ -1818,7 +1803,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
CDev("MPU6000_gyro", path),
_parent(parent),
_gyro_topic(-1),
_gyro_orb_id(nullptr),
_gyro_orb_class_instance(-1),
_gyro_class_instance(-1)
{
}
src/drivers/ms5611/ms5611.cpp
+15 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -57,6 +57,7 @@
#include <nuttx/clock.h>
#include <arch/board/board.h>
#include <board_config.h>
#include <drivers/device/device.h>
#include <drivers/drv_baro.h>
@@ -134,8 +135,7 @@ protected:
unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
orb_id_t _orb_id;
int _orb_class_instance;
int _class_instance;
perf_counter_t _sample_perf;
@@ -171,6 +171,13 @@ protected:
*/
void cycle();
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ }
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
@@ -210,6 +217,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char*
_SENS(0),
_msl_pressure(101325),
_baro_topic(-1),
_orb_class_instance(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
@@ -263,7 +271,6 @@ MS5611::init()
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_DEVICE_PATH);
_orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance);
struct baro_report brp;
/* do a first measurement cycle to populate reports with valid data */
@@ -303,7 +310,9 @@ MS5611::init()
ret = OK;
_baro_topic = orb_advertise(_orb_id, &brp);
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
if (_baro_topic < 0) {
warnx("failed to create sensor_baro publication");
@@ -725,7 +734,7 @@ MS5611::collect()
/* publish it */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_orb_id, _baro_topic, &report);
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
}
if (_reports->force(&report)) {