diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 06e2616237..3ddb6b5e34 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 06e26162373848b9ceb9de8b9006a0ebe952ea6d +Subproject commit 3ddb6b5e34d870b487dbd57949a7d9c718730701 diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index f71f41d52f..a3d28f6d41 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -66,19 +66,19 @@ #include #include "SyncObj.hpp" -#include "VirtDriverObj.hpp" +#include "VirtDevObj.hpp" using namespace DriverFramework; #define ADC_BASE_DEV_PATH "/dev/adc" -class ADCSIM : public VirtDriverObj +class ADCSIM : public VirtDevObj { public: ADCSIM(uint32_t channels); virtual ~ADCSIM(); - static int read(DriverHandle &h, adc_msg_s *sample, size_t num_samples); + virtual ssize_t read(void *buffer, ssize_t len); private: WorkHandle _call; @@ -103,7 +103,7 @@ private: }; ADCSIM::ADCSIM(uint32_t channels) : - VirtDriverObj("adcsim", ADC_BASE_DEV_PATH, 10000), + VirtDevObj("adcsim", ADC_BASE_DEV_PATH, 10000), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) @@ -143,23 +143,21 @@ ADCSIM::~ADCSIM() } } -int ADCSIM::read(DriverHandle &h, adc_msg_s *sample, size_t num_samples) +ssize_t +ADCSIM::read(void *buffer, ssize_t len) { - ADCSIM *me = DriverMgr::getDriverObjByHandle(h); - if (me) { - if (num_samples > me->_channel_count) { - num_samples = me->_channel_count; - } - size_t len = num_samples * sizeof(adc_msg_s); - - /* block interrupts while copying samples to avoid racing with an update */ - me->m_lock.lock(); - memcpy((void *)sample, (void *)(me->_samples), len); - me->m_lock.unlock(); - return num_samples; + const size_t maxsize = sizeof(adc_msg_s) * _channel_count; + + if (len > maxsize) { + len = maxsize; } + + /* block interrupts while copying samples to avoid racing with an update */ + m_lock.lock(); + memcpy(buffer, _samples, len); + m_lock.unlock(); - return -1; + return len; } void @@ -196,7 +194,8 @@ ADCSIM *g_adc; int test(void) { - DriverHandle h = DriverMgr::getHandle(ADCSIM0_DEVICE_PATH); + DevHandle h; + DevMgr::getHandle(ADCSIM0_DEVICE_PATH, h); if (!h.isValid()) { PX4_ERR("can't open ADCSIM device (%d)", h.getError()); @@ -205,7 +204,7 @@ test(void) for (unsigned i = 0; i < 50; i++) { adc_msg_s data[12]; - ssize_t count = ADCSIM::read(h, data, sizeof(data)); + ssize_t count = h.read(data, sizeof(data)); if (count < 0) { PX4_ERR("read error (%d)", h.getError()); @@ -221,7 +220,7 @@ test(void) usleep(500000); } - DriverMgr::releaseHandle(h); + DevMgr::releaseHandle(h); return 0; } } diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 0ac023285c..430e13a00a 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -69,13 +69,17 @@ #include #include -#include +//#include #include -#include -#include +//#include +//#include #include #include +#include "VirtDevObj.hpp" + +using namespace DriverFramework; + #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -114,16 +118,17 @@ */ class GYROSIM_gyro; -class GYROSIM : public device::VDev +class GYROSIM : public VirtDevObj { public: GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation); virtual ~GYROSIM(); - virtual int init(); + int init(); + virtual int start(); - 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); int transfer(uint8_t *send, uint8_t *recv, unsigned len); /** @@ -136,14 +141,14 @@ public: protected: friend class GYROSIM_gyro; - virtual ssize_t gyro_read(device::file_t *filp, char *buffer, size_t buflen); - virtual int gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t gyro_read(void *buffer, size_t buflen); + virtual int gyro_ioctl(unsigned long cmd, void *arg); private: GYROSIM_gyro *_gyro; uint8_t _product; /** product code */ - struct hrt_call _call; + WorkHandle _call; unsigned _call_interval; ringbuffer::RingBuffer *_accel_reports; @@ -175,16 +180,6 @@ private: // last temperature reading for print_info() float _last_temperature; - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - /** * Reset chip. * @@ -192,21 +187,10 @@ private: */ int reset(); - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - /** * Fetch measurements from the sensor and update the report buffers. */ - void measure(); + virtual void _measure(); /** * Read a register from the GYROSIM @@ -286,19 +270,21 @@ private: #pragma pack(pop) uint8_t _regdata[108]; + + bool _pub_blocked = true; // Don't publish until initialized }; /** * Helper class implementing the gyro driver node. */ -class GYROSIM_gyro : public device::VDev +class GYROSIM_gyro : public VirtDevObj { public: GYROSIM_gyro(GYROSIM *parent, const char *path); - ~GYROSIM_gyro(); + virtual ~GYROSIM_gyro(); - 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); virtual int init(); @@ -307,6 +293,7 @@ protected: void parent_poll_notify(); + virtual void _measure(); private: GYROSIM *_parent; orb_advert_t _gyro_topic; @@ -322,11 +309,9 @@ private: extern "C" { __EXPORT int gyrosim_main(int argc, char *argv[]); } GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation) : - VDev("GYROSIM", path_accel), + VirtDevObj("GYROSIM", path_accel, 1000), _gyro(new GYROSIM_gyro(this, path_gyro)), _product(GYROSIMES_REV_C4), - _call{}, - _call_interval(0), _accel_reports(nullptr), _accel_scale{}, _accel_range_scale(0.0f), @@ -349,17 +334,12 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _rotation(rotation), _last_temperature(0) { - // disable debug() calls - _debug_enabled = false; - // Don't publish until initialized - _pub_blocked = true; - - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; + m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; /* Prime _gyro with parents devid. */ - _gyro->_device_id.devid = _device_id.devid; - _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_GYROSIM; + _gyro->m_id.dev_id = m_id.dev_id; + _gyro->m_id.dev_id_s.devtype = DRV_GYR_DEVTYPE_GYROSIM; // default accel scale factors _accel_scale.x_offset = 0; @@ -376,8 +356,6 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; - - memset(&_call, 0, sizeof(_call)); } GYROSIM::~GYROSIM() @@ -397,10 +375,6 @@ GYROSIM::~GYROSIM() delete _gyro_reports; } - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } - /* delete the perf counter */ perf_free(_sample_perf); perf_free(_accel_reads); @@ -413,15 +387,6 @@ GYROSIM::init() { int ret; - /* do VDevinit first */ - ret = VDev::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - PX4_WARN("VDev setup failed"); - return ret; - } - struct accel_report arp = {}; struct gyro_report grp = {}; @@ -467,13 +432,13 @@ GYROSIM::init() /* if probe/setup failed, bail now */ if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); + PX4_ERR("gyro init failed"); return ret; } - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + start(); - measure(); + _measure(); /* advertise sensor topic, measure manually to initialize valid report */ _accel_reports->get(&arp); @@ -571,13 +536,11 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) _sample_rate = 1000 / div; PX4_INFO("GYROSIM: Changed sample rate to %uHz", _sample_rate); - _call_interval = 1000000 / _sample_rate; - hrt_cancel(&_call); - hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); + setSampleInterval(1000000 / _sample_rate); } ssize_t -GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM::devRead(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(accel_report); @@ -589,7 +552,7 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { _accel_reports->flush(); - measure(); + _measure(); } /* if no data, error (we could block here) */ @@ -620,7 +583,7 @@ int GYROSIM::self_test() { if (perf_event_count(_sample_perf) == 0) { - measure(); + _measure(); } /* return 0 on success, 1 else */ @@ -724,7 +687,7 @@ GYROSIM::gyro_self_test() } ssize_t -GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM::gyro_read(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(gyro_report); @@ -736,7 +699,7 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { _gyro_reports->flush(); - measure(); + _measure(); } /* if no data, error (we could block here) */ @@ -764,15 +727,17 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) } int -GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM::devIOCTL(unsigned long cmd, void *arg) { + unsigned long ul_arg = (unsigned long)arg; + switch (cmd) { case SENSORIOCRESET: return reset(); case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (ul_arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -789,15 +754,15 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(filp, SENSORIOCSPOLLRATE, 1000); + return devIOCTL(SENSORIOCSPOLLRATE, (void *)1000); case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); + return devIOCTL(SENSORIOCSPOLLRATE, (void *)GYROSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + unsigned ticks = 1000000 / ul_arg; /* check against maximum sane rate */ if (ticks < 1000) { @@ -829,11 +794,11 @@ GYROSIM::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 (!_accel_reports->resize(arg)) { + if (!_accel_reports->resize(ul_arg)) { return -ENOMEM; } @@ -847,7 +812,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return _sample_rate; case ACCELIOCSSAMPLERATE: - _set_sample_rate(arg); + _set_sample_rate(ul_arg); return OK; case ACCELIOCSLOWPASS: @@ -855,7 +820,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; + struct accel_scale *s = (struct accel_scale *) ul_arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -869,11 +834,11 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((struct accel_scale *) ul_arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: - return set_accel_range(arg); + return set_accel_range(ul_arg); case ACCELIOCGRANGE: return (unsigned long)((_accel_range_m_s2) / GYROSIM_ONE_G + 0.5f); @@ -883,28 +848,30 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } int -GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM::gyro_ioctl(unsigned long cmd, void *arg) { + unsigned long ul_arg = (unsigned long)arg; + switch (cmd) { /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: - return ioctl(filp, cmd, arg); + return devIOCTL(cmd, 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 (!_gyro_reports->resize(arg)) { + if (!_gyro_reports->resize(ul_arg)) { return -ENOMEM; } @@ -918,7 +885,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) return _sample_rate; case GYROIOCSSAMPLERATE: - _set_sample_rate(arg); + _set_sample_rate(ul_arg); return OK; case GYROIOCSLOWPASS: @@ -949,7 +916,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) default: /* give it to the superclass */ - return VDev::ioctl(filp, cmd, arg); + return VirtDevObj::devIOCTL(cmd, arg); } } @@ -1020,7 +987,7 @@ GYROSIM::set_accel_range(unsigned max_g_in) return OK; } -void +int GYROSIM::start() { /* make sure we are stopped first */ @@ -1031,28 +998,11 @@ GYROSIM::start() _gyro_reports->flush(); /* start polling at the specified rate */ - if (_call_interval > 0) { - hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); - } + return DevObj::start(); } void -GYROSIM::stop() -{ - hrt_cancel(&_call); -} - -void -GYROSIM::measure_trampoline(void *arg) -{ - GYROSIM *dev = reinterpret_cast(arg); - - /* make another measurement */ - dev->measure(); -} - -void -GYROSIM::measure() +GYROSIM::_measure() { #if 0 @@ -1152,8 +1102,9 @@ GYROSIM::measure() /* notify anyone waiting for data */ - poll_notify(POLLIN); - _gyro->parent_poll_notify(); + // FIXME + //poll_notify(POLLIN); + //_gyro->parent_poll_notify(); if (!(_pub_blocked)) { /* log the time of this report */ @@ -1211,7 +1162,7 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : - VDev("GYROSIM_gyro", path), + VirtDevObj("GYROSIM_gyro", path, 0), _parent(parent), _gyro_topic(nullptr), _gyro_orb_class_instance(-1), @@ -1219,55 +1170,37 @@ GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : { } -GYROSIM_gyro::~GYROSIM_gyro() -{ - if (_gyro_class_instance != -1) { - unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); - } -} - int GYROSIM_gyro::init() { - int ret; - - // do base class init - ret = VDev::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); - return ret; - } - - _gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); - - return ret; + return start(); } +#if 0 void GYROSIM_gyro::parent_poll_notify() { poll_notify(POLLIN); } +#endif ssize_t -GYROSIM_gyro::read(device::file_t *filp, char *buffer, size_t buflen) +GYROSIM_gyro::devRead(void *buffer, size_t buflen) { - return _parent->gyro_read(filp, buffer, buflen); + return _parent->gyro_read(buffer, buflen); } int -GYROSIM_gyro::ioctl(device::file_t *filp, int cmd, unsigned long arg) +GYROSIM_gyro::devIOCTL(unsigned long cmd, void *arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VDev::ioctl(filp, cmd, arg); + return (int)VirtDevObj::devIOCTL(cmd, arg); break; default: - return _parent->gyro_ioctl(filp, cmd, arg); + return _parent->gyro_ioctl(cmd, arg); } } @@ -1375,29 +1308,31 @@ test() ssize_t sz; /* get the driver */ - int fd = px4_open(path_accel, O_RDONLY); + DevHandle h_accel; + DevMgr::getHandle(path_accel, h_accel); - if (fd < 0) { + if (!h_accel.isValid()) { PX4_ERR("%s open failed (try 'gyrosim start')", path_accel); return 1; } /* get the driver */ - int fd_gyro = px4_open(path_gyro, O_RDONLY); + DevHandle h_gyro; + DevMgr::getHandle(path_gyro, h_gyro); - if (fd_gyro < 0) { + if (!h_gyro.isValid()) { PX4_ERR("%s open failed", path_gyro); return 1; } /* reset to manual polling */ - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { + if (h_accel.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_MANUAL) < 0) { PX4_ERR("reset to manual polling"); return 1; } /* do a simple demand read */ - sz = px4_read(fd, &a_report, sizeof(a_report)); + sz = h_accel.read(&a_report, sizeof(a_report)); if (sz != sizeof(a_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(a_report)); @@ -1417,7 +1352,7 @@ test() (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); /* do a simple demand read */ - sz = px4_read(fd_gyro, &g_report, sizeof(g_report)); + sz = h_gyro.read(&g_report, sizeof(g_report)); if (sz != sizeof(g_report)) { PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(g_report)); @@ -1440,11 +1375,12 @@ test() /* XXX add poll-rate tests here too */ - px4_close(fd); + // Destructor would clean the up too + DevMgr::releaseHandle(h_accel); + DevMgr::releaseHandle(h_gyro); reset(); PX4_INFO("PASS"); - return 0; } @@ -1454,30 +1390,28 @@ test() int reset() { - const char *path_accel = MPU_DEVICE_PATH_ACCEL; - int fd = px4_open(path_accel, O_RDONLY); + DevHandle h; + DevMgr::getHandle(MPU_DEVICE_PATH_ACCEL, h); - if (fd < 0) { + if (!h.isValid()) { PX4_ERR("reset failed"); return 1; } - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { + if (h.ioctl(SENSORIOCRESET, (void *)0) < 0) { PX4_ERR("driver reset failed"); goto reset_fail; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("driver poll restart failed"); goto reset_fail; } - px4_close(fd); return 0; reset_fail: - px4_close(fd); return 1; }