From efd9995a10e7e508be8bc0a82ac7d71f0d689a44 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 10 Nov 2015 07:45:13 -0800 Subject: [PATCH] Ported more simulated sensors to DriverFramework Signed-off-by: Mark Charlebois --- src/drivers/device/vdev.cpp | 16 +- src/drivers/drv_device.h | 6 + src/lib/DriverFramework | 2 +- .../commander/state_machine_helper.cpp | 28 ++-- .../posix/drivers/accelsim/accelsim.cpp | 28 ++-- src/platforms/posix/drivers/barosim/baro.cpp | 153 +++++++++--------- .../posix/drivers/barosim/baro_sim.cpp | 6 +- src/platforms/posix/drivers/barosim/barosim.h | 15 +- .../posix/drivers/gyrosim/gyrosim.cpp | 2 +- .../posix/drivers/tonealrmsim/tone_alarm.cpp | 73 ++++----- 10 files changed, 168 insertions(+), 161 deletions(-) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 60761ed511..12aa08b154 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -45,6 +45,10 @@ #include #include +#include "DevMgr.hpp" + +using namespace DriverFramework; + namespace device { @@ -512,13 +516,21 @@ VDev *VDev::getDev(const char *path) void VDev::showDevices() { int i = 0; - PX4_INFO("Devices:"); + PX4_INFO("PX4 Devices:"); for (; i < PX4_MAX_DEV; ++i) { if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) { 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() @@ -566,4 +578,4 @@ const char *VDev::devList(unsigned int *next) return NULL; } -} // namespace device \ No newline at end of file +} // namespace device diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index 02dbe7af67..af0f4982b3 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -46,6 +46,7 @@ #include "drv_sensor.h" #include "drv_orb_dev.h" +#ifdef __PX4_NUTTX /* * ioctl() definitions */ @@ -65,6 +66,11 @@ */ #define DEVIOCGDEVICEID _DEVICEIOC(2) +#else +#include "DevObj.hpp" + +#endif + #ifdef __PX4_POSIX #ifndef SIOCDEVPRIVATE diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index c4be5e70b3..0d91f67f2e 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit c4be5e70b36c86868520472f93fd945d7f0d8986 +Subproject commit 0d91f67f2e8a1ae91a5c853d5cae8b30e67a635e diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21789c9c66..6ac5046ac7 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -65,6 +65,10 @@ #include "state_machine_helper.h" #include "commander_helper.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 // 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 - const char *devname; - unsigned int handle = 0; + std::string devname; + unsigned int index = 0; for(;;) { - devname = px4_get_device_names(&handle); - if (devname == NULL) + if (DevMgr::getNextDeviceName(index, devname) < 0) { break; + } /* skip mavlink */ - if (!strcmp("/dev/mavlink", devname)) { + if (!strcmp("/dev/mavlink", devname.c_str())) { continue; } + DevHandle h; + DevMgr::getHandle(devname.c_str(), h); - int sensfd = px4_open(devname, 0); - - if (sensfd < 0) { - warn("failed opening device %s", devname); + if (!h.isValid()) { + warn("failed opening device %s", devname.c_str()); continue; } - int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1); - px4_close(sensfd); + int block_ret = h.ioctl(DEVIOCSPUBBLOCK, (void *)1); + 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; diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index ca231bffb9..7a82cfdff3 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -53,7 +53,6 @@ #include #include -#include #include #include #include @@ -244,10 +243,8 @@ public: ACCELSIM_mag(ACCELSIM *parent); ~ACCELSIM_mag(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - - virtual int init(); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, void *arg); protected: friend class ACCELSIM; @@ -913,7 +910,7 @@ ACCELSIM::_measure() /* notify anyone waiting for data */ DevMgr::updateNotify(*this); - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { /* publish it */ // 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); /* notify anyone waiting for data */ - DevMgr::updateNotify(*this) + DevMgr::updateNotify(*this); - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); } @@ -1014,7 +1011,7 @@ ACCELSIM::mag_measure() } 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), _mag_topic(nullptr), _mag_orb_class_instance(-1), @@ -1027,7 +1024,7 @@ ACCELSIM_mag::~ACCELSIM_mag() } ssize_t -ACCELSIM_mag::devRead(char *buffer, size_t buflen) +ACCELSIM_mag::devRead(void *buffer, size_t buflen) { return _parent->mag_read(buffer, buflen); } @@ -1037,7 +1034,7 @@ ACCELSIM_mag::devIOCTL(unsigned long cmd, void *arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VirtDevObj::ioctl(cmd, arg); + return (int)VirtDevObj::devIOCTL(cmd, arg); break; default: @@ -1077,6 +1074,9 @@ start(enum Rotation rotation) return 0; } + DevHandle h; + DevHandle h_mag; + /* create the driver */ 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 */ - DevHandle h; DevMgr::getHandle(ACCELSIM_DEVICE_PATH_ACCEL, h); if (!h.isValid()) { @@ -1099,18 +1098,17 @@ start(enum Rotation rotation) 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); DevMgr::releaseHandle(h); goto fail; } - DevHandle h_mag; DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag); /* don't fail if mag dev cannot be opened */ 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); } diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index 975fa3687c..02b9bb9183 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include @@ -67,6 +66,7 @@ #include #include "barosim.h" +#include "VirtDevObj.hpp" enum BAROSIM_BUS { BAROSIM_BUS_SIM_EXTERNAL @@ -86,16 +86,16 @@ enum BAROSIM_BUS { #define BAROSIM_CONVERSION_INTERVAL 10000 /* microseconds */ #define BAROSIM_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -class BAROSIM : public device::VDev +class BAROSIM : public VirtDevObj { 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(); virtual int init(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t devRead(void *buffer, size_t buflen); + virtual int devIOCTL(unsigned long cmd, void *arg); /** * Diagnostics - print some basic information about the driver. @@ -103,7 +103,7 @@ public: void print_info(); protected: - Device *_interface; + DevObj *_interface; barosim::prom_s _prom; @@ -182,7 +182,10 @@ protected: * * @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. @@ -195,8 +198,8 @@ protected: */ extern "C" __EXPORT int barosim_main(int argc, char *argv[]); -BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path) : - VDev("BAROSIM", path), +BAROSIM::BAROSIM(DevObj *interface, barosim::prom_u &prom_buf, const char *path) : + VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, 0), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), @@ -224,10 +227,6 @@ BAROSIM::~BAROSIM() /* make sure we are truly inactive */ stop_cycle(); - if (_class_instance != -1) { - unregister_class_devname(get_devname(), _class_instance); - } - /* free any existing reports */ if (_reports != nullptr) { delete _reports; @@ -246,12 +245,12 @@ int BAROSIM::init() { int ret; - DEVICE_DEBUG("BAROSIM::init"); + //PX4_DEBUG("BAROSIM::init"); - ret = VDev::init(); + ret = VirtDevObj::init(); if (ret != OK) { - DEVICE_DEBUG("VDev init failed"); + PX4_ERR("VirtDevObj init failed"); goto out; } @@ -259,14 +258,11 @@ BAROSIM::init() _reports = new ringbuffer::RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { - DEVICE_DEBUG("can't get memory for reports"); + PX4_ERR("can't get memory for reports"); ret = -ENOMEM; goto out; } - /* register alternate interfaces if we have to */ - _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); - struct baro_report brp; /* do a first measurement cycle to populate reports with valid data */ _measure_phase = 0; @@ -325,7 +321,7 @@ out: } 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); struct baro_report *brp = reinterpret_cast(buffer); @@ -397,12 +393,14 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } 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) { case SENSORIOCSPOLLRATE: - switch (arg) { + switch (ul_arg) { /* switching to manual polling */ 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); /* convert hz to tick interval via microseconds */ - unsigned long ticks = USEC2TICK(1000000 / arg); + unsigned long ticks = USEC2TICK(1000000 / ul_arg); /* check against maximum rate */ if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) { @@ -468,11 +466,11 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) { + if ((ul_arg < 1) || (ul_arg > 100)) { return -EINVAL; } - if (!_reports->resize(arg)) { + if (!_reports->resize(ul_arg)) { return -ENOMEM; } @@ -492,11 +490,11 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case BAROIOCSMSLPRESSURE: /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) { + if ((ul_arg < 80000) || (ul_arg > 120000)) { return -EINVAL; } - _msl_pressure = arg; + _msl_pressure = ul_arg; return OK; case BAROIOCGMSLPRESSURE: @@ -508,7 +506,7 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* give it to the bus-specific superclass */ // return bus_ioctl(filp, cmd, arg); - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } void @@ -542,7 +540,7 @@ void BAROSIM::cycle() { int ret; - unsigned dummy; + void *dummy = nullptr; /* collection phase? */ if (_collect_phase) { @@ -552,7 +550,7 @@ BAROSIM::cycle() if (ret != OK) { /* 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 */ start_cycle(); return; @@ -586,7 +584,7 @@ BAROSIM::cycle() if (ret != OK) { //DEVICE_LOG("measure error %d", ret); /* 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 */ start_cycle(); return; @@ -613,12 +611,12 @@ BAROSIM::measure() /* * 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. */ - ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); + ret = _interface->devIOCTL(IOCTL_MEASURE, (void *)addr); if (OK != ret) { perf_count(_comms_errors); @@ -650,7 +648,7 @@ BAROSIM::collect() report.error_count = perf_event_count(_comms_errors); /* 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) { perf_count(_comms_errors); @@ -672,7 +670,7 @@ BAROSIM::collect() report.timestamp = hrt_absolute_time(); /* publish it */ - if (!(_pub_blocked)) { + if (!(m_pub_blocked)) { if (_baro_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); @@ -687,7 +685,7 @@ BAROSIM::collect() } /* notify anyone waiting for data */ - poll_notify(POLLIN); + DevMgr::updateNotify(*this); } /* update the measurement state machine */ @@ -810,7 +808,7 @@ start_bus(struct barosim_bus_option &bus) } 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) { delete interface; @@ -827,21 +825,22 @@ start_bus(struct barosim_bus_option &bus) 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 */ - if (fd == -1) { + if (!h.isValid()) { PX4_ERR("can't open baro device"); return false; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - px4_close(fd); + if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + DevMgr::releaseHandle(h); PX4_ERR("failed setting default poll rate"); return false; } - px4_close(fd); + DevMgr::releaseHandle(h); return true; } @@ -882,17 +881,16 @@ test() ssize_t sz; int ret; - int fd; + DevHandle h; + DevMgr::getHandle(bus.devpath, h); - fd = px4_open(bus.devpath, O_RDONLY); - - if (fd < 0) { - PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); + if (!h.isValid()) { + PX4_ERR("getHandle failed (try 'barosim start' if the driver is not running)"); return 1; } /* do a simple demand read */ - sz = px4_read(fd, &report, sizeof(report)); + sz = h.read(&report, sizeof(report)); if (sz != sizeof(report)) { PX4_ERR("immediate read failed"); @@ -906,38 +904,37 @@ test() PX4_INFO("time: %lld", (long long)report.timestamp); /* 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_close(fd); + DevMgr::releaseHandle(h); return 1; } /* 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_close(fd); + DevMgr::releaseHandle(h); return 1; } /* read the sensor 5x and report each value */ 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 */ - fds.fd = fd; - fds.events = POLLIN; - ret = px4_poll(&fds, 1, 2000); + ret = DevMgr::waitForUpdate(in_set, out_set, 1000); if (ret != 1) { PX4_WARN("timed out waiting for sensor data"); } /* now go get it */ - sz = px4_read(fd, &report, sizeof(report)); + sz = h.read(&report, sizeof(report)); if (sz != sizeof(report)) { PX4_ERR("periodic read failed"); - px4_close(fd); + DevMgr::releaseHandle(h); return 1; } @@ -948,7 +945,7 @@ test() PX4_INFO("time: %lld", (long long)report.timestamp); } - px4_close(fd); + DevMgr::releaseHandle(h); PX4_INFO("PASS"); return 0; } @@ -960,21 +957,21 @@ int reset() { 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 "); return 1; } - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + if (h.ioctl(SENSORIOCRESET, 0) < 0) { PX4_ERR("driver reset failed"); 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"); return 1; } @@ -1008,20 +1005,19 @@ calibrate(unsigned altitude) { struct barosim_bus_option &bus = bus_options[0]; struct baro_report report; - float pressure; - float p1; + float pressure; + float p1; - int fd; + DevHandle h; + DevMgr::getHandle(bus.devpath, h); - fd = px4_open(bus.devpath, O_RDONLY); - - if (fd < 0) { + if (!h.isValid()) { PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); return 1; } /* 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"); return 1; } @@ -1030,14 +1026,13 @@ calibrate(unsigned altitude) pressure = 0.0f; for (unsigned i = 0; i < 20; i++) { - px4_pollfd_struct_t fds; int ret; ssize_t sz; + UpdateList in_set, out_set; + in_set.push_back(&h); /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = px4_poll(&fds, 1, 1000); + ret = DevMgr::waitForUpdate(in_set, out_set, 1000); if (ret != 1) { PX4_ERR("timed out waiting for sensor data"); @@ -1045,7 +1040,7 @@ calibrate(unsigned altitude) } /* now go get it */ - sz = px4_read(fd, &report, sizeof(report)); + sz = h.read(&report, sizeof(report)); if (sz != sizeof(report)) { PX4_ERR("sensor read failed"); @@ -1073,12 +1068,12 @@ calibrate(unsigned altitude) /* save as integer Pa */ p1 *= 1000.0f; - if (px4_ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)p1) != OK) { + if (h.ioctl(BAROIOCSMSLPRESSURE, (void *)((unsigned long)(p1))) != OK) { PX4_WARN("BAROIOCSMSLPRESSURE"); return 1; } - px4_close(fd); + DevMgr::releaseHandle(h); return 0; } diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 252d1428c9..5fc38aaa1c 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -54,7 +54,7 @@ #include "barosim.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 { @@ -94,10 +94,10 @@ private: }; -device::Device * +DevObj * BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum) { - return new BARO_SIM(busnum, prom_buf); + return reinterpret_cast(new BARO_SIM(busnum, prom_buf)); } BARO_SIM::BARO_SIM(uint8_t bus, barosim::prom_u &prom) : diff --git a/src/platforms/posix/drivers/barosim/barosim.h b/src/platforms/posix/drivers/barosim/barosim.h index bac176d400..db7f009a51 100644 --- a/src/platforms/posix/drivers/barosim/barosim.h +++ b/src/platforms/posix/drivers/barosim/barosim.h @@ -36,13 +36,14 @@ * * 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_D2 0x58 /* write to this address to start temperature conversion */ -#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_C1 0xA2 /* address of 6x 2 bytes calibration 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_C1 0xA2 /* address of 6x 2 bytes calibration data */ /* interface ioctls */ #define IOCTL_RESET 2 @@ -79,6 +80,8 @@ extern bool crc4(uint16_t *n_prom); } /* namespace */ +using namespace DriverFramework; + /* interface factories */ -extern device::Device *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum); -typedef device::Device *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum); +extern DevObj *BAROSIM_sim_interface(barosim::prom_u &prom_buf, uint8_t busnum); +typedef DevObj *(*BAROSIM_constructor)(barosim::prom_u &prom_buf, uint8_t busnum); diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 52e3254b7f..151777de6b 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -426,7 +426,7 @@ GYROSIM::init() _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(); /* if probe/setup failed, bail now */ diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index b58cccb5bd..a1ef3b3624 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -109,17 +109,18 @@ #include +#include "VirtDevObj.hpp" -class ToneAlarm : public device::VDev +using namespace DriverFramework; + +class ToneAlarm : public VirtDevObj { public: ToneAlarm(); ~ToneAlarm(); - virtual int init(); - - 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); + virtual int devIOCTL(unsigned long cmd, void *arg); + virtual ssize_t devWrite(const void *buffer, size_t len); inline const char *name(int tune) { return _tune_names[tune]; @@ -196,6 +197,8 @@ private: // static void next_trampoline(void *arg); + // Unused + virtual void _measure() {} }; // 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() : - VDev("tone_alarm", TONEALARM0_DEVICE_PATH), + VirtDevObj("tone_alarm", TONEALARM0_DEVICE_PATH, nullptr, 0), _default_tune_number(0), _user_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 ToneAlarm::note_to_divisor(unsigned note) { @@ -653,21 +641,21 @@ ToneAlarm::next_trampoline(void *arg) int -ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) +ToneAlarm::devIOCTL(unsigned long cmd, void *arg) { 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 */ switch (cmd) { 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 (arg == TONE_STOP_TUNE) { + if (ul_arg < TONE_NUMBER_OF_TUNES) { + if (ul_arg == TONE_STOP_TUNE) { // stop the tune _tune = nullptr; _next = nullptr; @@ -676,10 +664,10 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) } else { /* 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 */ - _default_tune_number = arg; - start_tune(_default_tunes[arg]); + _default_tune_number = ul_arg; + start_tune(_default_tunes[ul_arg]); } } @@ -694,18 +682,16 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } -// irqrestore(flags); - /* give it to the superclass if we didn't like it */ if (result == -ENOTTY) { - result = VDev::ioctl(filp, cmd, arg); + result = VirtDevObj::devIOCTL(cmd, arg); } return result; } 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 if (len > _tune_max) { @@ -726,13 +712,15 @@ ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) _user_tune = nullptr; } + const char *buf = reinterpret_cast(buffer); + // if the new tune is empty, we're done - if (buffer[0] == '\0') { + if (buf[0] == '\0') { return OK; } // allocate a copy of the new tune - _user_tune = strndup(buffer, len); + _user_tune = strndup(buf, len); if (_user_tune == nullptr) { return -ENOMEM; @@ -778,17 +766,18 @@ play_tune(unsigned tune) int 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) { - PX4_WARN("Error: failed to open %s", TONEALARM0_DEVICE_PATH); + if (!h.isValid()) { + PX4_WARN("Error: failed to get handle to %s", TONEALARM0_DEVICE_PATH); return 1; } - ret = px4_write(fd, str, strlen(str) + 1); - px4_close(fd); + ret = h.write(str, strlen(str) + 1); + DevMgr::releaseHandle(h); if (free_buffer) { free((void *)str);