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

Ported more simulated sensors to DriverFramework

Browse Source 
Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-11-10 07:45:13 -08:00
parent 40c079efa8
commit efd9995a10
10 changed files with 168 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/device/vdev.cpp
+14 -2
View File
@@ -45,6 +45,10 @@
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include "DevMgr.hpp"
using namespace DriverFramework;
namespace device namespace device
{ {
@@ -512,13 +516,21 @@ VDev *VDev::getDev(const char *path)
void VDev::showDevices() void VDev::showDevices()
{ {
int i = 0; int i = 0;
PX4_INFO("Devices:"); PX4_INFO("PX4 Devices:");
for (; i < PX4_MAX_DEV; ++i) { for (; i < PX4_MAX_DEV; ++i) {
if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) { if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
PX4_INFO(" %s", devmap[i]->name); PX4_INFO(" %s", devmap[i]->name);
} }
} }
PX4_INFO("DF Devices:");
std::string devname;
for (unsigned int index=0; i == 0; ++i) {
if (DevMgr::getNextDeviceName(index, devname) == 0) {
PX4_INFO(" %s", devname.c_str());
}
}
} }
void VDev::showTopics() void VDev::showTopics()
@@ -566,4 +578,4 @@ const char *VDev::devList(unsigned int *next)
return NULL; return NULL;
} }
} // namespace device } // namespace device
src/drivers/drv_device.h
+6
View File
@@ -46,6 +46,7 @@
#include "drv_sensor.h" #include "drv_sensor.h"
#include "drv_orb_dev.h" #include "drv_orb_dev.h"
#ifdef __PX4_NUTTX
/* /*
* ioctl() definitions * ioctl() definitions
*/ */
@@ -65,6 +66,11 @@
*/ */
#define DEVIOCGDEVICEID _DEVICEIOC(2) #define DEVIOCGDEVICEID _DEVICEIOC(2)
#else
#include "DevObj.hpp"
#endif
#ifdef __PX4_POSIX #ifdef __PX4_POSIX
#ifndef SIOCDEVPRIVATE #ifndef SIOCDEVPRIVATE
src/lib/DriverFramework
+1 -1
Submodule src/lib/DriverFramework updated: c4be5e70b3...0d91f67f2e
src/modules/commander/state_machine_helper.cpp
+16 -12
View File
@@ -65,6 +65,10 @@
#include "state_machine_helper.h" #include "state_machine_helper.h"
#include "commander_helper.h" #include "commander_helper.h"
#include "PreflightCheck.h" #include "PreflightCheck.h"
#ifndef __PX4_NUTTX
#include "DevMgr.hpp"
using namespace DriverFramework;
#endif
// This array defines the arming state transitions. The rows are the new state, and the columns // This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which // are the current state. Using new state and current state you can index into the array which
@@ -463,30 +467,30 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
#else #else
const char *devname; std::string devname;
unsigned int handle = 0; unsigned int index = 0;
for(;;) { for(;;) {
devname = px4_get_device_names(&handle); if (DevMgr::getNextDeviceName(index, devname) < 0) {
if (devname == NULL)
break; break;
}
/* skip mavlink */ /* skip mavlink */
if (!strcmp("/dev/mavlink", devname)) { if (!strcmp("/dev/mavlink", devname.c_str())) {
continue; continue;
} }
DevHandle h;
DevMgr::getHandle(devname.c_str(), h);
int sensfd = px4_open(devname, 0); if (!h.isValid()) {
warn("failed opening device %s", devname.c_str());
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue; continue;
} }
int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); int block_ret = h.ioctl(DEVIOCSPUBBLOCK, (void *)1);
px4_close(sensfd); DevMgr::releaseHandle(h);
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); printf("Disabling %s: %s\n", devname.c_str(), (block_ret == OK) ? "OK" : "ERROR");
} }
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
src/platforms/posix/drivers/accelsim/accelsim.cpp
+13 -15
View File
@@ -53,7 +53,6 @@
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <drivers/device/device.h>
#include <drivers/drv_accel.h> #include <drivers/drv_accel.h>
#include <drivers/drv_mag.h> #include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@@ -244,10 +243,8 @@ public:
ACCELSIM_mag(ACCELSIM *parent); ACCELSIM_mag(ACCELSIM *parent);
~ACCELSIM_mag(); ~ACCELSIM_mag();
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); virtual ssize_t devRead(void *buffer, size_t buflen);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual int devIOCTL(unsigned long cmd, void *arg);
virtual int init();
protected: protected:
friend class ACCELSIM; friend class ACCELSIM;
@@ -913,7 +910,7 @@ ACCELSIM::_measure()
/* notify anyone waiting for data */ /* notify anyone waiting for data */
DevMgr::updateNotify(*this); DevMgr::updateNotify(*this);
if (!(_pub_blocked)) { if (!(m_pub_blocked)) {
/* publish it */ /* publish it */
// The first call to measure() is from init() and _accel_topic is not // The first call to measure() is from init() and _accel_topic is not
@@ -1000,9 +997,9 @@ ACCELSIM::mag_measure()
_mag_reports->force(&mag_report); _mag_reports->force(&mag_report);
/* notify anyone waiting for data */ /* notify anyone waiting for data */
DevMgr::updateNotify(*this) DevMgr::updateNotify(*this);
if (!(_pub_blocked)) { if (!(m_pub_blocked)) {
/* publish it */ /* publish it */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
} }
@@ -1014,7 +1011,7 @@ ACCELSIM::mag_measure()
} }
ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) :
VirtDev("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG, MAG_BASE_DEVICE_PATH, 10000), VirtDevObj("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG, MAG_BASE_DEVICE_PATH, 10000),
_parent(parent), _parent(parent),
_mag_topic(nullptr), _mag_topic(nullptr),
_mag_orb_class_instance(-1), _mag_orb_class_instance(-1),
@@ -1027,7 +1024,7 @@ ACCELSIM_mag::~ACCELSIM_mag()
} }
ssize_t ssize_t
ACCELSIM_mag::devRead(char *buffer, size_t buflen) ACCELSIM_mag::devRead(void *buffer, size_t buflen)
{ {
return _parent->mag_read(buffer, buflen); return _parent->mag_read(buffer, buflen);
} }
@@ -1037,7 +1034,7 @@ ACCELSIM_mag::devIOCTL(unsigned long cmd, void *arg)
{ {
switch (cmd) { switch (cmd) {
case DEVIOCGDEVICEID: case DEVIOCGDEVICEID:
return (int)VirtDevObj::ioctl(cmd, arg); return (int)VirtDevObj::devIOCTL(cmd, arg);
break; break;
default: default:
@@ -1077,6 +1074,9 @@ start(enum Rotation rotation)
return 0; return 0;
} }
DevHandle h;
DevHandle h_mag;
/* create the driver */ /* create the driver */
g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation); g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation);
@@ -1091,7 +1091,6 @@ start(enum Rotation rotation)
} }
/* set the poll rate to default, starts automatic data collection */ /* set the poll rate to default, starts automatic data collection */
DevHandle h;
DevMgr::getHandle(ACCELSIM_DEVICE_PATH_ACCEL, h); DevMgr::getHandle(ACCELSIM_DEVICE_PATH_ACCEL, h);
if (!h.isValid()) { if (!h.isValid()) {
@@ -1099,18 +1098,17 @@ start(enum Rotation rotation)
goto fail; goto fail;
} }
if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
DevMgr::releaseHandle(h); DevMgr::releaseHandle(h);
goto fail; goto fail;
} }
DevHandle h_mag;
DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag); DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag);
/* don't fail if mag dev cannot be opened */ /* don't fail if mag dev cannot be opened */
if (!h_mag.isValid()) { if (!h_mag.isValid()) {
if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL);
} }
src/platforms/posix/drivers/barosim/baro.cpp
+74 -79
View File
@@ -48,7 +48,6 @@
#include <semaphore.h> #include <semaphore.h>
#include <string.h> #include <string.h>
#include <fcntl.h> #include <fcntl.h>
#include <poll.h>
#include <errno.h> #include <errno.h>
#include <stdio.h> #include <stdio.h>
#include <math.h> #include <math.h>
@@ -67,6 +66,7 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include "barosim.h" #include "barosim.h"
#include "VirtDevObj.hpp"
enum BAROSIM_BUS { enum BAROSIM_BUS {
BAROSIM_BUS_SIM_EXTERNAL BAROSIM_BUS_SIM_EXTERNAL
@@ -86,16 +86,16 @@ enum BAROSIM_BUS {
#define BAROSIM_CONVERSION_INTERVAL 10000 /* microseconds */ #define BAROSIM_CONVERSION_INTERVAL 10000 /* microseconds */
#define BAROSIM_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ #define BAROSIM_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */
class BAROSIM : public device::VDev class BAROSIM : public VirtDevObj
{ {
public: public:
BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path); BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path);
~BAROSIM(); ~BAROSIM();
virtual int init(); virtual int init();
virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); virtual ssize_t devRead(void *buffer, size_t buflen);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual int devIOCTL(unsigned long cmd, void *arg);
/** /**
* Diagnostics - print some basic information about the driver. * Diagnostics - print some basic information about the driver.
@@ -103,7 +103,7 @@ public:
void print_info(); void print_info();
protected: protected:
Device *_interface; DevObj *_interface;
barosim::prom_s _prom; barosim::prom_s _prom;
@@ -182,7 +182,10 @@ protected:
* *
* @return OK if the measurement command was successful. * @return OK if the measurement command was successful.
*/ */
virtual int measure(); int measure();
// Unused
virtual void _measure() {}
/** /**
* Collect the result of the most recent measurement. * Collect the result of the most recent measurement.
@@ -195,8 +198,8 @@ protected:
*/ */
extern "C" __EXPORT int barosim_main(int argc, char *argv[]); extern "C" __EXPORT int barosim_main(int argc, char *argv[]);
BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path) : BAROSIM::BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path) :
VDev("BAROSIM", path), VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 0),
_interface(interface), _interface(interface),
_prom(prom_buf.s), _prom(prom_buf.s),
_measure_ticks(0), _measure_ticks(0),
@@ -224,10 +227,6 @@ BAROSIM::~BAROSIM()
/* make sure we are truly inactive */ /* make sure we are truly inactive */
stop_cycle(); stop_cycle();
if (_class_instance != -1) {
unregister_class_devname(get_devname(), _class_instance);
}
/* free any existing reports */ /* free any existing reports */
if (_reports != nullptr) { if (_reports != nullptr) {
delete _reports; delete _reports;
@@ -246,12 +245,12 @@ int
BAROSIM::init() BAROSIM::init()
{ {
int ret; int ret;
DEVICE_DEBUG("BAROSIM::init"); //PX4_DEBUG("BAROSIM::init");
ret = VDev::init(); ret = VirtDevObj::init();
if (ret != OK) { if (ret != OK) {
DEVICE_DEBUG("VDev init failed"); PX4_ERR("VirtDevObj init failed");
goto out; goto out;
} }
@@ -259,14 +258,11 @@ BAROSIM::init()
_reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report));
if (_reports == nullptr) { if (_reports == nullptr) {
DEVICE_DEBUG("can't get memory for reports"); PX4_ERR("can't get memory for reports");
ret = -ENOMEM; ret = -ENOMEM;
goto out; goto out;
} }
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_BASE_DEVICE_PATH);
struct baro_report brp; struct baro_report brp;
/* do a first measurement cycle to populate reports with valid data */ /* do a first measurement cycle to populate reports with valid data */
_measure_phase = 0; _measure_phase = 0;
@@ -325,7 +321,7 @@ out:
} }
ssize_t ssize_t
BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) BAROSIM::devRead(void *buffer, size_t buflen)
{ {
unsigned count = buflen / sizeof(struct baro_report); unsigned count = buflen / sizeof(struct baro_report);
struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer); struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer);
@@ -397,12 +393,14 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen)
} }
int int
BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) BAROSIM::devIOCTL(unsigned long cmd, void *arg)
{ {
unsigned long ul_arg = (unsigned long)arg;
switch (cmd) { switch (cmd) {
case SENSORIOCSPOLLRATE: case SENSORIOCSPOLLRATE:
switch (arg) { switch (ul_arg) {
/* switching to manual polling */ /* switching to manual polling */
case SENSOR_POLLRATE_MANUAL: case SENSOR_POLLRATE_MANUAL:
@@ -440,7 +438,7 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
bool want_start = (_measure_ticks == 0); bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */ /* convert hz to tick interval via microseconds */
unsigned long ticks = USEC2TICK(1000000 / arg); unsigned long ticks = USEC2TICK(1000000 / ul_arg);
/* check against maximum rate */ /* check against maximum rate */
if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) { if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) {
@@ -468,11 +466,11 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) { if ((ul_arg < 1) || (ul_arg > 100)) {
return -EINVAL; return -EINVAL;
} }
if (!_reports->resize(arg)) { if (!_reports->resize(ul_arg)) {
return -ENOMEM; return -ENOMEM;
} }
@@ -492,11 +490,11 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
case BAROIOCSMSLPRESSURE: case BAROIOCSMSLPRESSURE:
/* range-check for sanity */ /* range-check for sanity */
if ((arg < 80000) || (arg > 120000)) { if ((ul_arg < 80000) || (ul_arg > 120000)) {
return -EINVAL; return -EINVAL;
} }
_msl_pressure = arg; _msl_pressure = ul_arg;
return OK; return OK;
case BAROIOCGMSLPRESSURE: case BAROIOCGMSLPRESSURE:
@@ -508,7 +506,7 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg)
/* give it to the bus-specific superclass */ /* give it to the bus-specific superclass */
// return bus_ioctl(filp, cmd, arg); // return bus_ioctl(filp, cmd, arg);
return VDev::ioctl(filp, cmd, arg); return VirtDevObj::devIOCTL(cmd, arg);
} }
void void
@@ -542,7 +540,7 @@ void
BAROSIM::cycle() BAROSIM::cycle()
{ {
int ret; int ret;
unsigned dummy; void *dummy = nullptr;
/* collection phase? */ /* collection phase? */
if (_collect_phase) { if (_collect_phase) {
@@ -552,7 +550,7 @@ BAROSIM::cycle()
if (ret != OK) { if (ret != OK) {
/* issue a reset command to the sensor */ /* issue a reset command to the sensor */
_interface->dev_ioctl(IOCTL_RESET, dummy); _interface->devIOCTL(IOCTL_RESET, (void *)dummy);
/* reset the collection state machine and try again */ /* reset the collection state machine and try again */
start_cycle(); start_cycle();
return; return;
@@ -586,7 +584,7 @@ BAROSIM::cycle()
if (ret != OK) { if (ret != OK) {
//DEVICE_LOG("measure error %d", ret); //DEVICE_LOG("measure error %d", ret);
/* issue a reset command to the sensor */ /* issue a reset command to the sensor */
_interface->dev_ioctl(IOCTL_RESET, dummy); _interface->devIOCTL(IOCTL_RESET, dummy);
/* reset the collection state machine and try again */ /* reset the collection state machine and try again */
start_cycle(); start_cycle();
return; return;
@@ -613,12 +611,12 @@ BAROSIM::measure()
/* /*
* In phase zero, request temperature; in other phases, request pressure. * In phase zero, request temperature; in other phases, request pressure.
*/ */
unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; unsigned long addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1;
/* /*
* Send the command to begin measuring. * Send the command to begin measuring.
*/ */
ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); ret = _interface->devIOCTL(IOCTL_MEASURE, (void *)addr);
if (OK != ret) { if (OK != ret) {
perf_count(_comms_errors); perf_count(_comms_errors);
@@ -650,7 +648,7 @@ BAROSIM::collect()
report.error_count = perf_event_count(_comms_errors); report.error_count = perf_event_count(_comms_errors);
/* read the most recent measurement - read offset/size are hardcoded in the interface */ /* read the most recent measurement - read offset/size are hardcoded in the interface */
ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report)); ret = _interface->devRead((void *)&baro_report, sizeof(baro_report));
if (ret < 0) { if (ret < 0) {
perf_count(_comms_errors); perf_count(_comms_errors);
@@ -672,7 +670,7 @@ BAROSIM::collect()
report.timestamp = hrt_absolute_time(); report.timestamp = hrt_absolute_time();
/* publish it */ /* publish it */
if (!(_pub_blocked)) { if (!(m_pub_blocked)) {
if (_baro_topic != nullptr) { if (_baro_topic != nullptr) {
/* publish it */ /* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
@@ -687,7 +685,7 @@ BAROSIM::collect()
} }
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); DevMgr::updateNotify(*this);
} }
/* update the measurement state machine */ /* update the measurement state machine */
@@ -810,7 +808,7 @@ start_bus(struct barosim_bus_option &bus)
} }
prom_u prom_buf; prom_u prom_buf;
device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); DevObj *interface = bus.interface_constructor(prom_buf, bus.busnum);
if (interface->init() != OK) { if (interface->init() != OK) {
delete interface; delete interface;
@@ -827,21 +825,22 @@ start_bus(struct barosim_bus_option &bus)
return false; return false;
} }
int fd = px4_open(bus.devpath, O_RDONLY); DevHandle h;
DevMgr::getHandle(bus.devpath, h);
/* set the poll rate to default, starts automatic data collection */ /* set the poll rate to default, starts automatic data collection */
if (fd == -1) { if (!h.isValid()) {
PX4_ERR("can't open baro device"); PX4_ERR("can't open baro device");
return false; return false;
} }
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) {
px4_close(fd); DevMgr::releaseHandle(h);
PX4_ERR("failed setting default poll rate"); PX4_ERR("failed setting default poll rate");
return false; return false;
} }
px4_close(fd); DevMgr::releaseHandle(h);
return true; return true;
} }
@@ -882,17 +881,16 @@ test()
ssize_t sz; ssize_t sz;
int ret; int ret;
int fd; DevHandle h;
DevMgr::getHandle(bus.devpath, h);
fd = px4_open(bus.devpath, O_RDONLY); if (!h.isValid()) {
PX4_ERR("getHandle failed (try 'barosim start' if the driver is not running)");
if (fd < 0) {
PX4_ERR("open failed (try 'barosim start' if the driver is not running)");
return 1; return 1;
} }
/* do a simple demand read */ /* do a simple demand read */
sz = px4_read(fd, &report, sizeof(report)); sz = h.read(&report, sizeof(report));
if (sz != sizeof(report)) { if (sz != sizeof(report)) {
PX4_ERR("immediate read failed"); PX4_ERR("immediate read failed");
@@ -906,38 +904,37 @@ test()
PX4_INFO("time: %lld", (long long)report.timestamp); PX4_INFO("time: %lld", (long long)report.timestamp);
/* set the queue depth to 10 */ /* set the queue depth to 10 */
if (OK != px4_ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { if (OK != h.ioctl(SENSORIOCSQUEUEDEPTH, (void *)10UL)) {
PX4_ERR("failed to set queue depth"); PX4_ERR("failed to set queue depth");
px4_close(fd); DevMgr::releaseHandle(h);
return 1; return 1;
} }
/* start the sensor polling at 2Hz */ /* start the sensor polling at 2Hz */
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { if (OK != h.ioctl(SENSORIOCSPOLLRATE, (void *)2UL)) {
PX4_ERR("failed to set 2Hz poll rate"); PX4_ERR("failed to set 2Hz poll rate");
px4_close(fd); DevMgr::releaseHandle(h);
return 1; return 1;
} }
/* read the sensor 5x and report each value */ /* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) { for (unsigned i = 0; i < 5; i++) {
px4_pollfd_struct_t fds; UpdateList in_set, out_set;
in_set.push_back(&h);
/* wait for data to be ready */ /* wait for data to be ready */
fds.fd = fd; ret = DevMgr::waitForUpdate(in_set, out_set, 1000);
fds.events = POLLIN;
ret = px4_poll(&fds, 1, 2000);
if (ret != 1) { if (ret != 1) {
PX4_WARN("timed out waiting for sensor data"); PX4_WARN("timed out waiting for sensor data");
} }
/* now go get it */ /* now go get it */
sz = px4_read(fd, &report, sizeof(report)); sz = h.read(&report, sizeof(report));
if (sz != sizeof(report)) { if (sz != sizeof(report)) {
PX4_ERR("periodic read failed"); PX4_ERR("periodic read failed");
px4_close(fd); DevMgr::releaseHandle(h);
return 1; return 1;
} }
@@ -948,7 +945,7 @@ test()
PX4_INFO("time: %lld", (long long)report.timestamp); PX4_INFO("time: %lld", (long long)report.timestamp);
} }
px4_close(fd); DevMgr::releaseHandle(h);
PX4_INFO("PASS"); PX4_INFO("PASS");
return 0; return 0;
} }
@@ -960,21 +957,21 @@ int
reset() reset()
{ {
struct barosim_bus_option &bus = bus_options[0]; struct barosim_bus_option &bus = bus_options[0];
int fd;
fd = px4_open(bus.devpath, O_RDONLY); DevHandle h;
DevMgr::getHandle(bus.devpath, h);
if (fd < 0) { if (!h.isValid()) {
PX4_ERR("failed "); PX4_ERR("failed ");
return 1; return 1;
} }
if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { if (h.ioctl(SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed"); PX4_ERR("driver reset failed");
return 1; return 1;
} }
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("driver poll restart failed"); PX4_ERR("driver poll restart failed");
return 1; return 1;
} }
@@ -1008,20 +1005,19 @@ calibrate(unsigned altitude)
{ {
struct barosim_bus_option &bus = bus_options[0]; struct barosim_bus_option &bus = bus_options[0];
struct baro_report report; struct baro_report report;
float pressure; float pressure;
float p1; float p1;
int fd; DevHandle h;
DevMgr::getHandle(bus.devpath, h);
fd = px4_open(bus.devpath, O_RDONLY); if (!h.isValid()) {
if (fd < 0) {
PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); PX4_ERR("open failed (try 'barosim start' if the driver is not running)");
return 1; return 1;
} }
/* start the sensor polling at max */ /* start the sensor polling at max */
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) { if (OK != h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_MAX)) {
PX4_ERR("failed to set poll rate"); PX4_ERR("failed to set poll rate");
return 1; return 1;
} }
@@ -1030,14 +1026,13 @@ calibrate(unsigned altitude)
pressure = 0.0f; pressure = 0.0f;
for (unsigned i = 0; i < 20; i++) { for (unsigned i = 0; i < 20; i++) {
px4_pollfd_struct_t fds;
int ret; int ret;
ssize_t sz; ssize_t sz;
UpdateList in_set, out_set;
in_set.push_back(&h);
/* wait for data to be ready */ /* wait for data to be ready */
fds.fd = fd; ret = DevMgr::waitForUpdate(in_set, out_set, 1000);
fds.events = POLLIN;
ret = px4_poll(&fds, 1, 1000);
if (ret != 1) { if (ret != 1) {
PX4_ERR("timed out waiting for sensor data"); PX4_ERR("timed out waiting for sensor data");
@@ -1045,7 +1040,7 @@ calibrate(unsigned altitude)
} }
/* now go get it */ /* now go get it */
sz = px4_read(fd, &report, sizeof(report)); sz = h.read(&report, sizeof(report));
if (sz != sizeof(report)) { if (sz != sizeof(report)) {
PX4_ERR("sensor read failed"); PX4_ERR("sensor read failed");
@@ -1073,12 +1068,12 @@ calibrate(unsigned altitude)
/* save as integer Pa */ /* save as integer Pa */
p1 *= 1000.0f; p1 *= 1000.0f;
if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) { if (h.ioctl(BAROIOCSMSLPRESSURE, (void *)((unsigned long)(p1))) != OK) {
PX4_WARN("BAROIOCSMSLPRESSURE"); PX4_WARN("BAROIOCSMSLPRESSURE");
return 1; return 1;
} }
px4_close(fd); DevMgr::releaseHandle(h);
return 0; return 0;
} }
src/platforms/posix/drivers/barosim/baro_sim.cpp
+3 -3
View File
@@ -54,7 +54,7 @@
#include "barosim.h" #include "barosim.h"
#include "board_config.h" #include "board_config.h"
device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf); DevObj *BAROSIM_sim_interface(barosim::prom_u &prom_buf);
class BARO_SIM : public device::SIM class BARO_SIM : public device::SIM
{ {
@@ -94,10 +94,10 @@ private:
}; };
device::Device * DevObj *
BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum) BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum)
{ {
return new BARO_SIM(busnum, prom_buf); return reinterpret_cast<DevObj *>(new BARO_SIM(busnum, prom_buf));
} }
BARO_SIM::BARO_SIM(uint8_t bus, barosim::prom_u &prom) : BARO_SIM::BARO_SIM(uint8_t bus, barosim::prom_u &prom) :
src/platforms/posix/drivers/barosim/barosim.h
+9 -6
View File
@@ -36,13 +36,14 @@
* *
* Shared defines for the ms5611 driver. * Shared defines for the ms5611 driver.
*/ */
#include "DevObj.hpp"
#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ #define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */
#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */ #define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */
#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start temperature conversion */ #define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start temperature conversion */
#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ #define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */
#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ #define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */
#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ #define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */
/* interface ioctls */ /* interface ioctls */
#define IOCTL_RESET 2 #define IOCTL_RESET 2
@@ -79,6 +80,8 @@ extern bool crc4(uint16_t *n_prom);
} /* namespace */ } /* namespace */
using namespace DriverFramework;
/* interface factories */ /* interface factories */
extern device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum); extern DevObj *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum);
typedef device::Device *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum); typedef DevObj *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum);
src/platforms/posix/drivers/gyrosim/gyrosim.cpp
+1 -1
View File
@@ -426,7 +426,7 @@ GYROSIM::init()
_gyro_scale.z_scale = 1.0f; _gyro_scale.z_scale = 1.0f;
/* do VDev init for the gyro device node, keep it optional */ /* do init for the gyro device node, keep it optional */
ret = _gyro->init(); ret = _gyro->init();
/* if probe/setup failed, bail now */ /* if probe/setup failed, bail now */
src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp
+31 -42
View File
@@ -109,17 +109,18 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include "VirtDevObj.hpp"
class ToneAlarm : public device::VDev using namespace DriverFramework;
class ToneAlarm : public VirtDevObj
{ {
public: public:
ToneAlarm(); ToneAlarm();
~ToneAlarm(); ~ToneAlarm();
virtual int init(); virtual int devIOCTL(unsigned long cmd, void *arg);
virtual ssize_t devWrite(const void *buffer, size_t len);
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
virtual ssize_t write(device::file_t *filp, const char *buffer, size_t len);
inline const char *name(int tune) inline const char *name(int tune)
{ {
return _tune_names[tune]; return _tune_names[tune];
@@ -196,6 +197,8 @@ private:
// //
static void next_trampoline(void *arg); static void next_trampoline(void *arg);
// Unused
virtual void _measure() {}
}; };
// semitone offsets from C for the characters 'A'-'G' // semitone offsets from C for the characters 'A'-'G'
@@ -208,7 +211,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
ToneAlarm::ToneAlarm() : ToneAlarm::ToneAlarm() :
VDev("tone_alarm", TONEALARM0_DEVICE_PATH), VirtDevObj("tone_alarm", TONEALARM0_DEVICE_PATH, nullptr, 0),
_default_tune_number(0), _default_tune_number(0),
_user_tune(nullptr), _user_tune(nullptr),
_tune(nullptr), _tune(nullptr),
@@ -251,21 +254,6 @@ ToneAlarm::~ToneAlarm()
{ {
} }
int
ToneAlarm::init()
{
int ret;
ret = VDev::init();
if (ret != OK) {
return ret;
}
DEVICE_DEBUG("ready");
return OK;
}
unsigned unsigned
ToneAlarm::note_to_divisor(unsigned note) ToneAlarm::note_to_divisor(unsigned note)
{ {
@@ -653,21 +641,21 @@ ToneAlarm::next_trampoline(void *arg)
int int
ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) ToneAlarm::devIOCTL(unsigned long cmd, void *arg)
{ {
int result = OK; int result = OK;
DEVICE_DEBUG("ioctl %i %lu", cmd, arg); unsigned long ul_arg = (unsigned long)arg;
// irqstate_t flags = irqsave(); PX4_DEBUG("ToneAlarm::devIOCTL %i %lu", cmd, ul_arg);
/* decide whether to increase the alarm level to cmd or leave it alone */ /* decide whether to increase the alarm level to cmd or leave it alone */
switch (cmd) { switch (cmd) {
case TONE_SET_ALARM: case TONE_SET_ALARM:
PX4_INFO("TONE_SET_ALARM %lu", arg); PX4_INFO("TONE_SET_ALARM %lu", ul_arg);
if (arg < TONE_NUMBER_OF_TUNES) { if (ul_arg < TONE_NUMBER_OF_TUNES) {
if (arg == TONE_STOP_TUNE) { if (ul_arg == TONE_STOP_TUNE) {
// stop the tune // stop the tune
_tune = nullptr; _tune = nullptr;
_next = nullptr; _next = nullptr;
@@ -676,10 +664,10 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg)
} else { } else {
/* always interrupt alarms, unless they are repeating and already playing */ /* always interrupt alarms, unless they are repeating and already playing */
if (!(_repeat && _default_tune_number == arg)) { if (!(_repeat && _default_tune_number == ul_arg)) {
/* play the selected tune */ /* play the selected tune */
_default_tune_number = arg; _default_tune_number = ul_arg;
start_tune(_default_tunes[arg]); start_tune(_default_tunes[ul_arg]);
} }
} }
@@ -694,18 +682,16 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg)
break; break;
} }
// irqrestore(flags);
/* give it to the superclass if we didn't like it */ /* give it to the superclass if we didn't like it */
if (result == -ENOTTY) { if (result == -ENOTTY) {
result = VDev::ioctl(filp, cmd, arg); result = VirtDevObj::devIOCTL(cmd, arg);
} }
return result; return result;
} }
ssize_t ssize_t
ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) ToneAlarm::devWrite(const void *buffer, size_t len)
{ {
// sanity-check the buffer for length and nul-termination // sanity-check the buffer for length and nul-termination
if (len > _tune_max) { if (len > _tune_max) {
@@ -726,13 +712,15 @@ ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len)
_user_tune = nullptr; _user_tune = nullptr;
} }
const char *buf = reinterpret_cast<const char *>(buffer);
// if the new tune is empty, we're done // if the new tune is empty, we're done
if (buffer[0] == '\0') { if (buf[0] == '\0') {
return OK; return OK;
} }
// allocate a copy of the new tune // allocate a copy of the new tune
_user_tune = strndup(buffer, len); _user_tune = strndup(buf, len);
if (_user_tune == nullptr) { if (_user_tune == nullptr) {
return -ENOMEM; return -ENOMEM;
@@ -778,17 +766,18 @@ play_tune(unsigned tune)
int int
play_string(const char *str, bool free_buffer) play_string(const char *str, bool free_buffer)
{ {
int fd, ret; int ret;
fd = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY); DevHandle h;
DevMgr::getHandle(TONEALARM0_DEVICE_PATH, h);
if (fd < 0) { if (!h.isValid()) {
PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH); PX4_WARN("Error: failed to get handle to %s", TONEALARM0_DEVICE_PATH);
return 1; return 1;
} }
ret = px4_write(fd, str, strlen(str) + 1); ret = h.write(str, strlen(str) + 1);
px4_close(fd); DevMgr::releaseHandle(h);
if (free_buffer) { if (free_buffer) {
free((void *)str); free((void *)str);