From d04a4969e211a2fad5d2c18fdb266a39bd5d773a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sun, 1 Nov 2015 23:43:46 -0800 Subject: [PATCH 01/24] Integrated DriverFramework Refacored ADCSIM Compilation succeeds. Link not yet fixed. Signed-off-by: Mark Charlebois --- .gitmodules | 3 + cmake/posix/px4_impl_posix.cmake | 1 + src/lib/DriverFramework | 1 + src/platforms/posix/drivers/adcsim/adcsim.cpp | 123 +++++------------- 4 files changed, 41 insertions(+), 87 deletions(-) create mode 160000 src/lib/DriverFramework diff --git a/.gitmodules b/.gitmodules index 178cf86771..d47e011047 100644 --- a/.gitmodules +++ b/.gitmodules @@ -25,3 +25,6 @@ [submodule "unittests/googletest"] path = unittests/googletest url = https://github.com/google/googletest.git +[submodule "src/lib/DriverFramework"] + path = src/lib/DriverFramework + url = https://github.com/mcharleb/DriverFramework.git diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake index d51b5527d5..7dbf128780 100644 --- a/cmake/posix/px4_impl_posix.cmake +++ b/cmake/posix/px4_impl_posix.cmake @@ -164,6 +164,7 @@ function(px4_os_add_flags) src/lib/eigen src/platforms/posix/include mavlink/include/mavlink + src/lib/DriverFramework/framework/include ) if(UNIX AND APPLE) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework new file mode 160000 index 0000000000..06e2616237 --- /dev/null +++ b/src/lib/DriverFramework @@ -0,0 +1 @@ +Subproject commit 06e26162373848b9ceb9de8b9006a0ebe952ea6d diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index d9f0dd4f15..6d725344bf 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -65,36 +65,33 @@ #include +#include "SyncObj.hpp" +#include "VirtDriverObj.hpp" -class ADCSIM : public device::VDev +using namespace DriverFramework; + +#define ADC_BASE_DEV_PATH "/dev/adc" + +class ADCSIM : public VirtDriverObj { public: ADCSIM(uint32_t channels); - ~ADCSIM(); - - virtual int init(); + virtual ~ADCSIM(); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t len); - -protected: - virtual int open_first(device::file_t *filp); - virtual int close_last(device::file_t *filp); + static int read(DriverHandle &h, adc_msg_s *sample, size_t num_samples); private: - static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - - hrt_call _call; + WorkHandle _call; perf_counter_t _sample_perf; unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ /** work trampoline */ - static void _tick_trampoline(void *arg); + static void _tick_trampoline(void *arg, WorkHandle &h); /** worker function */ - void _tick(); + virtual void _measure(); /** * Sample a single channel and return the measured value. @@ -105,12 +102,11 @@ private: */ uint16_t _sample(unsigned channel); - // update system_power ORB topic, only on FMUv2 - void update_system_power(void); + SyncObj m_lock; }; ADCSIM::ADCSIM(uint32_t channels) : - VDev("adcsim", ADCSIM0_DEVICE_PATH), + VirtDriverObj("adcsim", ADC_BASE_DEV_PATH, 10000), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) @@ -150,75 +146,34 @@ ADCSIM::~ADCSIM() } } -int -ADCSIM::init() -{ - DEVICE_DEBUG("init done"); - - /* create the device node */ - return VDev::init(); -} - -int -ADCSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +int ADCSIM::read(DriverHandle &h, adc_msg_s *sample, size_t num_samples) { - return -ENOTTY; -} - -ssize_t -ADCSIM::read(device::file_t *filp, char *buffer, size_t len) -{ - const size_t maxsize = sizeof(adc_msg_s) * _channel_count; + 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); - if (len > maxsize) { - len = maxsize; + /* 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; } - /* block interrupts while copying samples to avoid racing with an update */ - memcpy(buffer, _samples, len); - - return len; -} - -int -ADCSIM::open_first(device::file_t *filp) -{ - /* get fresh data */ - _tick(); - - /* and schedule regular updates */ - hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); - - return 0; -} - -int -ADCSIM::close_last(device::file_t *filp) -{ - hrt_cancel(&_call); - return 0; -} - -void -ADCSIM::_tick_trampoline(void *arg) -{ - (reinterpret_cast(arg))->_tick(); + return -1; } void -ADCSIM::_tick() +ADCSIM::_measure() { + m_lock.lock(); /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); } - - update_system_power(); -} - -void -ADCSIM::update_system_power(void) -{ + m_lock.unlock(); } uint16_t @@ -245,19 +200,19 @@ int test(void) { - int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY); + DriverHandle h = DriverMgr::getHandle(ADCSIM0_DEVICE_PATH); - if (fd < 0) { - PX4_ERR("can't open ADCSIM device"); + if (!h.isValid()) { + PX4_ERR("can't open ADCSIM device (%d)", h.getError()); return 1; } for (unsigned i = 0; i < 50; i++) { adc_msg_s data[12]; - ssize_t count = px4_read(fd, data, sizeof(data)); + ssize_t count = ADCSIM::read(h, data, sizeof(data)); if (count < 0) { - PX4_ERR("read error"); + PX4_ERR("read error (%d)", h.getError()); return 1; } @@ -270,7 +225,7 @@ test(void) usleep(500000); } - px4_close(fd); + DriverMgr::releaseHandle(h); return 0; } } @@ -288,12 +243,6 @@ adcsim_main(int argc, char *argv[]) PX4_ERR("couldn't allocate the ADCSIM driver"); return 1; } - - if (g_adc->init() != OK) { - delete g_adc; - PX4_ERR("ADCSIM init failed"); - return 1; - } } if (argc > 1) { From 143fd4aec65724e6278734113a747a50e8a15df1 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 2 Nov 2015 00:06:12 -0800 Subject: [PATCH 02/24] Adding in DriverFramework Signed-off-by: Mark Charlebois --- src/firmware/posix/CMakeLists.txt | 1 + src/platforms/posix/drivers/adcsim/adcsim.cpp | 3 +-- src/platforms/posix/main.cpp | 8 +++++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 64f4775c33..4efaa2164a 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -14,6 +14,7 @@ if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" OR NOT APPLE) target_link_libraries(mainapp -Wl,--start-group ${module_libraries} + ${driver_framework_libraries} pthread m rt -Wl,--end-group ) diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 6d725344bf..0efbc94225 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -199,7 +199,6 @@ ADCSIM *g_adc; int test(void) { - DriverHandle h = DriverMgr::getHandle(ADCSIM0_DEVICE_PATH); if (!h.isValid()) { @@ -245,7 +244,7 @@ adcsim_main(int argc, char *argv[]) } } - if (argc > 1) { + if (argc > 1 && g_adc) { if (!strcmp(argv[1], "test")) { ret = test(); } diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 1b22627721..56301b603c 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -43,6 +43,9 @@ #include #include #include +#include "apps.h" +#include "px4_middleware.h" +#include "DriverFramework.hpp" namespace px4 { @@ -53,9 +56,6 @@ using namespace std; typedef int (*px4_main_t)(int argc, char *argv[]); -#include "apps.h" -#include "px4_middleware.h" - static bool _ExitFlag = false; extern "C" { void _SigIntHandler(int sig_num); @@ -184,6 +184,7 @@ int main(int argc, char **argv) } if (!error_detected) { + DriverFramework::Framework::initialize(); px4::init_once(); px4::init(argc, argv, "mainapp"); @@ -236,5 +237,6 @@ int main(int argc, char **argv) vector shutdown_cmd = { "shutdown" }; run_cmd(shutdown_cmd, true); + DriverFramework::Framework::shutdown(); } } From 1b7a6fffcaff8b08738d54e909e80e5990da2155 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 2 Nov 2015 00:11:34 -0800 Subject: [PATCH 03/24] Removed unussed function Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/adcsim/adcsim.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 0efbc94225..f71f41d52f 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -87,9 +87,6 @@ private: unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ - /** work trampoline */ - static void _tick_trampoline(void *arg, WorkHandle &h); - /** worker function */ virtual void _measure(); From ce97aea1bf999002f0b91629cb82db15469b6faf Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 2 Nov 2015 20:59:41 -0800 Subject: [PATCH 04/24] Added Gyrosim Signed-off-by: Mark Charlebois --- src/lib/DriverFramework | 2 +- src/platforms/posix/drivers/adcsim/adcsim.cpp | 41 ++- .../posix/drivers/gyrosim/gyrosim.cpp | 246 +++++++----------- 3 files changed, 111 insertions(+), 178 deletions(-) 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); - } -} - -void -GYROSIM::stop() -{ - hrt_cancel(&_call); -} - -void -GYROSIM::measure_trampoline(void *arg) -{ - GYROSIM *dev = reinterpret_cast(arg); - - /* make another measurement */ - dev->measure(); + return DevObj::start(); } 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; } From 596a2789d6c965c5fa63e9b10b240de35cb2ba41 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 3 Nov 2015 01:24:02 -0800 Subject: [PATCH 05/24] gyrosim and adcsim now compile with DriverFramework enabled Only the posix build is tested for compilation Signed-off-by: Mark Charlebois --- CMakeLists.txt | 4 +++ src/firmware/posix/CMakeLists.txt | 3 +- src/lib/DriverFramework | 2 +- src/platforms/posix/drivers/adcsim/adcsim.cpp | 5 ++-- .../posix/drivers/gyrosim/gyrosim.cpp | 29 +++++++++---------- 5 files changed, 22 insertions(+), 21 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 05a3cd59b3..7538d910a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,6 +121,9 @@ # and leads to wrong toolchain detection cmake_minimum_required(VERSION 2.8 FATAL_ERROR) +SET (CMAKE_C_COMPILER /usr/bin/clang-3.6) +SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6) + #============================================================================= # parameters # @@ -319,6 +322,7 @@ foreach(module ${config_module_list}) endforeach() add_subdirectory(src/firmware/${OS}) +add_subdirectory(src/lib/DriverFramework) if (config_io_board) add_subdirectory(src/modules/px4iofirmware) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 4efaa2164a..36bc434a77 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -14,7 +14,8 @@ if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" OR NOT APPLE) target_link_libraries(mainapp -Wl,--start-group ${module_libraries} - ${driver_framework_libraries} + df_driver_framework + ${df_driver_libraries} pthread m rt -Wl,--end-group ) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 3ddb6b5e34..c2afd87807 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 3ddb6b5e34d870b487dbd57949a7d9c718730701 +Subproject commit c2afd878071e8fb0b1d80070e44ba98780492fc1 diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index a3d28f6d41..66372ad214 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -78,10 +78,9 @@ public: ADCSIM(uint32_t channels); virtual ~ADCSIM(); - virtual ssize_t read(void *buffer, ssize_t len); + virtual ssize_t devRead(void *buffer, size_t len); private: - WorkHandle _call; perf_counter_t _sample_perf; unsigned _channel_count; @@ -144,7 +143,7 @@ ADCSIM::~ADCSIM() } ssize_t -ADCSIM::read(void *buffer, ssize_t len) +ADCSIM::devRead(void *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 430e13a00a..7aef8cf392 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -69,10 +69,10 @@ #include #include -//#include +#include #include -//#include -//#include +#include +#include #include #include @@ -277,11 +277,11 @@ private: /** * Helper class implementing the gyro driver node. */ -class GYROSIM_gyro : public VirtDevObj +class GYROSIM_gyro : public VirtDevObj { public: GYROSIM_gyro(GYROSIM *parent, const char *path); - virtual ~GYROSIM_gyro(); + virtual ~GYROSIM_gyro() {} virtual ssize_t devRead(void *buffer, size_t buflen); virtual int devIOCTL(unsigned long cmd, void *arg); @@ -293,12 +293,11 @@ protected: void parent_poll_notify(); - virtual void _measure(); + virtual void _measure() {}; private: GYROSIM *_parent; orb_advert_t _gyro_topic; int _gyro_orb_class_instance; - int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ GYROSIM_gyro(const GYROSIM_gyro &); @@ -385,7 +384,7 @@ GYROSIM::~GYROSIM() int GYROSIM::init() { - int ret; + int ret = 1; struct accel_report arp = {}; @@ -1102,9 +1101,8 @@ GYROSIM::_measure() /* notify anyone waiting for data */ - // FIXME - //poll_notify(POLLIN); - //_gyro->parent_poll_notify(); + updateNotify(); + _gyro->parent_poll_notify(); if (!(_pub_blocked)) { /* log the time of this report */ @@ -1162,27 +1160,26 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : + // Set sample interval to 0 since device is read by parent VirtDevObj("GYROSIM_gyro", path, 0), _parent(parent), _gyro_topic(nullptr), - _gyro_orb_class_instance(-1), - _gyro_class_instance(-1) + _gyro_orb_class_instance(-1) { } + int GYROSIM_gyro::init() { return start(); } -#if 0 void GYROSIM_gyro::parent_poll_notify() { - poll_notify(POLLIN); + updateNotify(); } -#endif ssize_t GYROSIM_gyro::devRead(void *buffer, size_t buflen) From 6e19d9cb841d2e0349b9ec40a4de70d78bd9b53f Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Tue, 3 Nov 2015 01:27:22 -0800 Subject: [PATCH 06/24] Commened out manditory use of Clang-3.6 Signed-off-by: Mark Charlebois --- CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7538d910a0..681bb8f328 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,8 +121,9 @@ # and leads to wrong toolchain detection cmake_minimum_required(VERSION 2.8 FATAL_ERROR) -SET (CMAKE_C_COMPILER /usr/bin/clang-3.6) -SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6) +# Use clang +#SET (CMAKE_C_COMPILER /usr/bin/clang-3.6) +#SET (CMAKE_CXX_COMPILER /usr/bin/clang++-3.6) #============================================================================= # parameters From a71f00686bcdfe0201e760349c2ba7df3ce457dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 11:06:28 +0100 Subject: [PATCH 07/24] Register the driver framework correctly --- CMakeLists.txt | 1 + src/platforms/posix/drivers/adcsim/CMakeLists.txt | 1 + src/platforms/posix/drivers/gyrosim/CMakeLists.txt | 1 + 3 files changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 681bb8f328..8893341009 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -234,6 +234,7 @@ px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest") px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan") px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") +px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework") px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo") diff --git a/src/platforms/posix/drivers/adcsim/CMakeLists.txt b/src/platforms/posix/drivers/adcsim/CMakeLists.txt index 93cdd95a0f..fc6802b0c4 100644 --- a/src/platforms/posix/drivers/adcsim/CMakeLists.txt +++ b/src/platforms/posix/drivers/adcsim/CMakeLists.txt @@ -39,5 +39,6 @@ px4_add_module( adcsim.cpp DEPENDS platforms__common + git_driverframework ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt index 335bee65d6..f776b34bea 100644 --- a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt @@ -40,5 +40,6 @@ px4_add_module( gyrosim.cpp DEPENDS platforms__common + git_driverframework ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : From a71f83e30fd3f85995d55975213da0b033a200be Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 11:53:26 +0100 Subject: [PATCH 08/24] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index c2afd87807..2c2e80b5cc 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit c2afd878071e8fb0b1d80070e44ba98780492fc1 +Subproject commit 2c2e80b5cc39ce5b96d3be19a1e1db3370ba7e85 From f4d9521fd67c54dc2962933db9a7b55d22ea300e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:05:19 +0100 Subject: [PATCH 09/24] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 2c2e80b5cc..dc52ed6d98 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 2c2e80b5cc39ce5b96d3be19a1e1db3370ba7e85 +Subproject commit dc52ed6d98ce007f86f6e0b7521636337753a239 From 97ee033fe0aa82a21b0e9566adb6355055a3cf85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:44:12 +0100 Subject: [PATCH 10/24] DriverFramework update --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index dc52ed6d98..080a7cdccd 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit dc52ed6d98ce007f86f6e0b7521636337753a239 +Subproject commit 080a7cdccd283cea79ce7908774391aac60a41fa From b1f2812862775d2e84082e37a35de2cc130b05bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:51:05 +0100 Subject: [PATCH 11/24] Updated framework URL --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index d47e011047..da67485fe8 100644 --- a/.gitmodules +++ b/.gitmodules @@ -27,4 +27,4 @@ url = https://github.com/google/googletest.git [submodule "src/lib/DriverFramework"] path = src/lib/DriverFramework - url = https://github.com/mcharleb/DriverFramework.git + url = https://github.com/PX4/DriverFramework.git From d9a7dd04a4b5c8625c9455914db364b5c0229e46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:52:04 +0100 Subject: [PATCH 12/24] Updated driver framework repo --- src/platforms/posix/drivers/adcsim/CMakeLists.txt | 6 ++++++ src/platforms/posix/drivers/gyrosim/CMakeLists.txt | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/src/platforms/posix/drivers/adcsim/CMakeLists.txt b/src/platforms/posix/drivers/adcsim/CMakeLists.txt index fc6802b0c4..c1c925c15c 100644 --- a/src/platforms/posix/drivers/adcsim/CMakeLists.txt +++ b/src/platforms/posix/drivers/adcsim/CMakeLists.txt @@ -40,5 +40,11 @@ px4_add_module( DEPENDS platforms__common git_driverframework + df_driver_framework ) + +target_link_libraries(platforms__posix__drivers__adcsim + df_driver_framework +) + # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt index f776b34bea..a1b42bd11c 100644 --- a/src/platforms/posix/drivers/gyrosim/CMakeLists.txt +++ b/src/platforms/posix/drivers/gyrosim/CMakeLists.txt @@ -41,5 +41,11 @@ px4_add_module( DEPENDS platforms__common git_driverframework + df_driver_framework ) + +target_link_libraries(platforms__posix__drivers__gyrosim + df_driver_framework +) + # vim: set noet ft=cmake fenc=utf-8 ff=unix : From 2ec6363f67408dcfb425d56aac59cdc211a7cc4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:56:19 +0100 Subject: [PATCH 13/24] Fixed adc sim formatting --- src/platforms/posix/drivers/adcsim/adcsim.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index 66372ad214..e8f1c592ec 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -146,11 +146,11 @@ ssize_t ADCSIM::devRead(void *buffer, size_t len) { 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); @@ -163,10 +163,12 @@ void ADCSIM::_measure() { m_lock.lock(); + /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); } + m_lock.unlock(); } From ed4e602511c3d86a1f67fc254dd54cb02461b4d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 12:57:44 +0100 Subject: [PATCH 14/24] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 080a7cdccd..e1582be979 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 080a7cdccd283cea79ce7908774391aac60a41fa +Subproject commit e1582be979cb903ef81ebd150b7667fde91763fe From 3b0f18e96d41f1d67866b71c5ce1e108db1159d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:13:25 +0100 Subject: [PATCH 15/24] Fix missing rt lib --- cmake/posix/px4_impl_posix.cmake | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/cmake/posix/px4_impl_posix.cmake b/cmake/posix/px4_impl_posix.cmake index 7dbf128780..28b27f87d1 100644 --- a/cmake/posix/px4_impl_posix.cmake +++ b/cmake/posix/px4_impl_posix.cmake @@ -176,6 +176,10 @@ if(UNIX AND APPLE) -include ${PX4_INCLUDE_DIR}visibility.h ) + set(added_exe_linker_flags + -lpthread + ) + else() set(added_definitions @@ -185,11 +189,12 @@ else() -Dnoreturn_function=__attribute__\(\(noreturn\)\) -include ${PX4_INCLUDE_DIR}visibility.h ) -endif() set(added_exe_linker_flags - -lpthread + -lpthread -lrt ) + +endif() # Add the toolchain specific flags From 8e7a4725c6d0406d3b2ed4104fec4af7e69d0053 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:14:26 +0100 Subject: [PATCH 16/24] Driverframework: Update --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index e1582be979..c07f902805 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit e1582be979cb903ef81ebd150b7667fde91763fe +Subproject commit c07f9028054bdad970c6283066703c59c4b3a35a From 28e08d31ad79c3e51698dea0c757ba97c295c0ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:16:28 +0100 Subject: [PATCH 17/24] Travis CI: Select CLANG --- .travis.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.travis.yml b/.travis.yml index e6b5239344..c94da476e4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,6 +3,8 @@ language: cpp +compiler: clang + matrix: fast_finish: true include: From 4e295e1549b44d58166b8b506fe24e9b4889deac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:17:27 +0100 Subject: [PATCH 18/24] Revert CLANG --- .travis.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index c94da476e4..e6b5239344 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,8 +3,6 @@ language: cpp -compiler: clang - matrix: fast_finish: true include: From 450ad27b52141d98d7acae771fd3d22ec9528a05 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:18:11 +0100 Subject: [PATCH 19/24] Update Driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index c07f902805..89d4bb5725 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit c07f9028054bdad970c6283066703c59c4b3a35a +Subproject commit 89d4bb57258dcfc78c48ed3466d77fdbc0b4471e From d9ba63a41f06010cedd9595d75b410cf769cdd25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:49:43 +0100 Subject: [PATCH 20/24] Add C++ flags --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 89d4bb5725..ca4970ab63 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 89d4bb57258dcfc78c48ed3466d77fdbc0b4471e +Subproject commit ca4970ab6329110a28272fb808539333a38dc173 From cf8e289bcec75b605956ec5deec68e38ea1e5bcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:54:06 +0100 Subject: [PATCH 21/24] Update driver framework --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index ca4970ab63..d58b46695b 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit ca4970ab6329110a28272fb808539333a38dc173 +Subproject commit d58b46695b702173459465f862d52a216229772b From 9c4f8f56acf92c41f141a3cd86995ff62ad0dceb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 13:57:29 +0100 Subject: [PATCH 22/24] Fix Cxx errors --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index d58b46695b..32786683a0 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit d58b46695b702173459465f862d52a216229772b +Subproject commit 32786683a0a0cbe3003fd55d6fc6c61999707b95 From 8b80a24d9d7d1159763d2b3fb208d4efb8f26fb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 6 Nov 2015 09:01:36 +0100 Subject: [PATCH 23/24] Update driver framework version --- src/lib/DriverFramework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 32786683a0..c4be5e70b3 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 32786683a0a0cbe3003fd55d6fc6c61999707b95 +Subproject commit c4be5e70b36c86868520472f93fd945d7f0d8986 From 40c079efa8f7d8cc402890384cb9ce4e01f6a1c7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 9 Nov 2015 10:26:28 -0800 Subject: [PATCH 24/24] Ported accelsim, gyrosim and adcsim to DriverFramework Need to handle _pub_blocked flag in DriverFramework as a PX4 specific implementation. Signed-off-by: Mark Charlebois --- .../posix/drivers/accelsim/accelsim.cpp | 250 +++++------------- src/platforms/posix/drivers/adcsim/adcsim.cpp | 2 +- .../posix/drivers/gyrosim/gyrosim.cpp | 20 +- 3 files changed, 84 insertions(+), 188 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 5017bb111c..ca231bffb9 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -44,7 +44,6 @@ #include #include #include -#include #include #include #include @@ -63,15 +62,9 @@ #include #include #include - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +#include #define ACCELSIM_DEVICE_PATH_ACCEL "/dev/sim_accel" -#define ACCELSIM_DEVICE_PATH_ACCEL_EXT "/dev/sim_accel_ext" #define ACCELSIM_DEVICE_PATH_MAG "/dev/sim_mag" #define ADDR_WHO_AM_I 0x0F @@ -87,10 +80,11 @@ static const int ERROR = -1; extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); } +using namespace DriverFramework; class ACCELSIM_mag; -class ACCELSIM : public device::VDev +class ACCELSIM : public VirtDevObj { public: ACCELSIM(const char *path, enum Rotation rotation); @@ -98,8 +92,8 @@ public: 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); /** * dump register values @@ -109,8 +103,8 @@ public: protected: friend class ACCELSIM_mag; - ssize_t mag_read(device::file_t *filp, char *buffer, size_t buflen); - int mag_ioctl(device::file_t *filp, int cmd, unsigned long arg); + ssize_t mag_read(void *buffer, size_t buflen); + int mag_ioctl(unsigned long cmd, void *arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); private: @@ -123,8 +117,8 @@ private: unsigned _call_accel_interval; unsigned _call_mag_interval; - ringbuffer::RingBuffer *_accel_reports; - ringbuffer::RingBuffer *_mag_reports; + ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -168,14 +162,9 @@ private: // reset /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. + * Override Start automatic measurement. */ - void stop(); + virtual int start(); /** * Reset chip. @@ -184,28 +173,10 @@ private: */ void 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); - - /** - * Static trampoline for the mag because it runs at a lower rate - * - * @param arg Instance pointer for the driver that is polling. - */ - static void mag_measure_trampoline(void *arg); - /** * Fetch accel measurements from the sensor and update the report ring. */ - void measure(); + virtual void _measure(); /** * Fetch mag measurements from the sensor and update the report ring. @@ -267,7 +238,7 @@ private: /** * Helper class implementing the mag driver node. */ -class ACCELSIM_mag : public device::VDev +class ACCELSIM_mag : public VirtDevObj { public: ACCELSIM_mag(ACCELSIM *parent); @@ -288,9 +259,7 @@ private: int _mag_orb_class_instance; int _mag_class_instance; - void measure(); - - void measure_trampoline(void *arg); + virtual void _measure(); /* this class does not allow copying due to ptr data members */ ACCELSIM_mag(const ACCELSIM_mag &); @@ -299,7 +268,7 @@ private: ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : - VDev("ACCELSIM", path), + VirtDevObj("ACCELSIM", path, ACCEL_BASE_DEVICE_PATH, 1000), _mag(new ACCELSIM_mag(this)), _accel_call{}, _mag_call{}, @@ -333,15 +302,11 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : _constant_accel_count(0), _last_temperature(0) { - // enable debug() calls - _debug_enabled = false; - - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; + m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; /* Prime _mag with parents devid. */ - _mag->_device_id.devid = _device_id.devid; - _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; - + _mag->m_id.dev_id = m_id.dev_id; + _mag->m_id.dev_id_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; // default scale factors _accel_scale.x_offset = 0.0f; @@ -373,10 +338,6 @@ ACCELSIM::~ACCELSIM() delete _mag_reports; } - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } - delete _mag; /* delete the perf counter */ @@ -390,13 +351,13 @@ ACCELSIM::~ACCELSIM() int ACCELSIM::init() { - int ret = ERROR; + int ret = -1; struct mag_report mrp = {}; struct accel_report arp = {}; /* do SIM init first */ - if (VDev::init() != OK) { + if (VirtDevObj::init() != 0) { PX4_WARN("SIM init failed"); goto out; } @@ -416,7 +377,7 @@ ACCELSIM::init() reset(); - /* do VDev init for the mag device node */ + /* do init for the mag device node */ ret = _mag->init(); if (ret != OK) { @@ -425,7 +386,7 @@ ACCELSIM::init() } /* fill report structures */ - measure(); + _measure(); /* advertise sensor topic, measure manually to initialize valid report */ _mag_reports->get(&mrp); @@ -438,9 +399,6 @@ ACCELSIM::init() PX4_WARN("ADVERT ERR"); } - - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); - /* advertise sensor topic, measure manually to initialize valid report */ _accel_reports->get(&arp); @@ -490,7 +448,7 @@ ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) } ssize_t -ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) +ACCELSIM::devRead(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); accel_report *arb = reinterpret_cast(buffer); @@ -518,7 +476,7 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* manual measurement */ - measure(); + _measure(); /* measurement will have generated a report, copy it out */ if (_accel_reports->get(arb)) { @@ -529,7 +487,7 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) } ssize_t -ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) +ACCELSIM::mag_read(void *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); mag_report *mrb = reinterpret_cast(buffer); @@ -559,7 +517,7 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) /* manual measurement */ _mag_reports->flush(); - _mag->measure(); + _mag->_measure(); /* measurement will have generated a report, copy it out */ if (_mag_reports->get(mrb)) { @@ -570,12 +528,13 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) } int -ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) +ACCELSIM::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: @@ -592,10 +551,10 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: - return ioctl(filp, SENSORIOCSPOLLRATE, 1600); + return devIOCTL(SENSORIOCSPOLLRATE, (void *)1600); case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); + return devIOCTL(SENSORIOCSPOLLRATE, (void *)ACCELSIM_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -603,7 +562,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_call_accel_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned period = 1000000 / arg; + unsigned period = 1000000 / ul_arg; /* check against maximum sane rate */ if (period < 500) { @@ -611,7 +570,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) } /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + accel_set_driver_lowpass_filter((float)ul_arg, _accel_filter_x.get_cutoff_freq()); /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ @@ -636,11 +595,11 @@ ACCELSIM::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; } @@ -655,13 +614,13 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return OK; case ACCELIOCSSAMPLERATE: - return accel_set_samplerate(arg); + return accel_set_samplerate(ul_arg); case ACCELIOCGSAMPLERATE: return _accel_samplerate; case ACCELIOCSLOWPASS: { - return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)ul_arg); } case ACCELIOCSSCALE: { @@ -680,7 +639,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCSRANGE: /* arg needs to be in G */ - return accel_set_range(arg); + return accel_set_range(ul_arg); case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ @@ -688,7 +647,7 @@ ACCELSIM::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 *) arg, &(_accel_scale), sizeof(_accel_scale)); return OK; case ACCELIOCSELFTEST: @@ -696,17 +655,18 @@ ACCELSIM::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 -ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) +ACCELSIM::mag_ioctl(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: @@ -725,7 +685,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: /* 100 Hz is max for mag */ - return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); + return mag_ioctl(SENSORIOCSPOLLRATE, (void *)100); /* adjust to a legal polling interval in Hz */ default: { @@ -733,7 +693,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_call_mag_interval == 0); /* convert hz to hrt interval via microseconds */ - unsigned period = 1000000 / arg; + unsigned period = 1000000 / ul_arg; /* check against maximum sane rate (1ms) */ if (period < 10000) { @@ -765,11 +725,11 @@ ACCELSIM::mag_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 (!_mag_reports->resize(arg)) { + if (!_mag_reports->resize(ul_arg)) { return -ENOMEM; } @@ -784,7 +744,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) return OK; case MAGIOCSSAMPLERATE: - return mag_set_samplerate(arg); + return mag_set_samplerate(ul_arg); case MAGIOCGSAMPLERATE: return _mag_samplerate; @@ -805,7 +765,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) return OK; case MAGIOCSRANGE: - return mag_set_range(arg); + return mag_set_range(ul_arg); case MAGIOCGRANGE: return _mag_range_ga; @@ -822,7 +782,7 @@ ACCELSIM::mag_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); } } @@ -868,7 +828,7 @@ ACCELSIM::mag_set_samplerate(unsigned frequency) return OK; } -void +int ACCELSIM::start() { /* make sure we are stopped first */ @@ -878,49 +838,11 @@ ACCELSIM::start() _accel_reports->flush(); _mag_reports->flush(); - /* start polling at the specified rate */ - //PX4_INFO("ACCELSIM::start accel %u", _call_accel_interval); - hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&ACCELSIM::measure_trampoline, this); - - // There is a race here where SENSORIOCSPOLLRATE on the accel starts polling of mag but _call_mag_interval is 0 - if (_call_mag_interval == 0) { - //PX4_INFO("_call_mag_interval uninitilized - would have set period delay of 0"); - _call_mag_interval = 10000; // Max 100Hz - } - - //PX4_INFO("ACCELSIM::start mag %u", _call_mag_interval); - hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&ACCELSIM::mag_measure_trampoline, this); + return VirtDevObj::start(); } void -ACCELSIM::stop() -{ - hrt_cancel(&_accel_call); - hrt_cancel(&_mag_call); -} - -void -ACCELSIM::measure_trampoline(void *arg) -{ - //PX4_INFO("ACCELSIM::measure_trampoline"); - ACCELSIM *dev = (ACCELSIM *)arg; - - /* make another measurement */ - dev->measure(); -} - -void -ACCELSIM::mag_measure_trampoline(void *arg) -{ - //PX4_INFO("ACCELSIM::mag_measure_trampoline"); - ACCELSIM *dev = (ACCELSIM *)arg; - - /* make another measurement */ - dev->mag_measure(); -} - -void -ACCELSIM::measure() +ACCELSIM::_measure() { /* status register and data as read back from the device */ @@ -989,7 +911,7 @@ ACCELSIM::measure() _accel_reports->force(&accel_report); /* notify anyone waiting for data */ - poll_notify(POLLIN); + DevMgr::updateNotify(*this); if (!(_pub_blocked)) { /* publish it */ @@ -1078,7 +1000,7 @@ ACCELSIM::mag_measure() _mag_reports->force(&mag_report); /* notify anyone waiting for data */ - poll_notify(POLLIN); + DevMgr::updateNotify(*this) if (!(_pub_blocked)) { /* publish it */ @@ -1092,7 +1014,7 @@ ACCELSIM::mag_measure() } ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : - VDev("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG), + VirtDev("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG, MAG_BASE_DEVICE_PATH, 10000), _parent(parent), _mag_topic(nullptr), _mag_orb_class_instance(-1), @@ -1102,59 +1024,33 @@ ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : ACCELSIM_mag::~ACCELSIM_mag() { - if (_mag_class_instance != -1) { - unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); - } -} - -int -ACCELSIM_mag::init() -{ - int ret; - - ret = VDev::init(); - - if (ret != OK) { - goto out; - } - - _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); - -out: - return ret; } ssize_t -ACCELSIM_mag::read(device::file_t *filp, char *buffer, size_t buflen) +ACCELSIM_mag::devRead(char *buffer, size_t buflen) { - return _parent->mag_read(filp, buffer, buflen); + return _parent->mag_read(buffer, buflen); } int -ACCELSIM_mag::ioctl(device::file_t *filp, int cmd, unsigned long arg) +ACCELSIM_mag::devIOCTL(unsigned long cmd, void *arg) { switch (cmd) { case DEVIOCGDEVICEID: - return (int)VDev::ioctl(filp, cmd, arg); + return (int)VirtDevObj::ioctl(cmd, arg); break; default: - return _parent->mag_ioctl(filp, cmd, arg); + return _parent->mag_ioctl(cmd, arg); } } void -ACCELSIM_mag::measure() +ACCELSIM_mag::_measure() { _parent->mag_measure(); } -void -ACCELSIM_mag::measure_trampoline(void *arg) -{ - _parent->mag_measure_trampoline(arg); -} - /** * Local functions in support of the shell command. */ @@ -1176,8 +1072,6 @@ void usage(); int start(enum Rotation rotation) { - int fd, fd_mag; - if (g_dev != nullptr) { PX4_WARN("already started"); return 0; @@ -1197,24 +1091,26 @@ start(enum Rotation rotation) } /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(ACCELSIM_DEVICE_PATH_ACCEL, O_RDONLY); + DevHandle h; + DevMgr::getHandle(ACCELSIM_DEVICE_PATH_ACCEL, h); - if (fd < 0) { + if (!h.isValid()) { PX4_WARN("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL); goto fail; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); - px4_close(fd); + DevMgr::releaseHandle(h); goto fail; } - fd_mag = px4_open(ACCELSIM_DEVICE_PATH_MAG, O_RDONLY); + DevHandle h_mag; + DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag); /* don't fail if mag dev cannot be opened */ - if (0 <= fd_mag) { - if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + if (!h_mag.isValid()) { + if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } @@ -1222,8 +1118,8 @@ start(enum Rotation rotation) PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } - px4_close(fd); - px4_close(fd_mag); + DevMgr::releaseHandle(h); + DevMgr::releaseHandle(h_mag); return 0; fail: diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index e8f1c592ec..4326314438 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -102,7 +102,7 @@ private: }; ADCSIM::ADCSIM(uint32_t channels) : - VirtDevObj("adcsim", ADC_BASE_DEV_PATH, 10000), + VirtDevObj("adcsim", "/dev/adcsim", ADC_BASE_DEV_PATH, 10000), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index 7aef8cf392..52e3254b7f 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -308,7 +308,7 @@ private: extern "C" { __EXPORT int gyrosim_main(int argc, char *argv[]); } GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation) : - VirtDevObj("GYROSIM", path_accel, 1000), + VirtDevObj("GYROSIM", path_accel, nullptr, 1000), _gyro(new GYROSIM_gyro(this, path_gyro)), _product(GYROSIMES_REV_C4), _accel_reports(nullptr), @@ -484,7 +484,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) // Get data from the simulator Simulator *sim = Simulator::getInstance(); - if (sim == NULL) { + if (sim == nullptr) { PX4_WARN("failed accessing simulator"); return ENODEV; } @@ -1161,7 +1161,7 @@ GYROSIM::print_registers() GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : // Set sample interval to 0 since device is read by parent - VirtDevObj("GYROSIM_gyro", path, 0), + VirtDevObj("GYROSIM_gyro", path, nullptr, 0), _parent(parent), _gyro_topic(nullptr), _gyro_orb_class_instance(-1) @@ -1226,10 +1226,10 @@ void usage(); int start(enum Rotation rotation) { - int fd; GYROSIM **g_dev_ptr = &g_dev_sim; const char *path_accel = MPU_DEVICE_PATH_ACCEL; const char *path_gyro = MPU_DEVICE_PATH_GYRO; + DevHandle h; if (*g_dev_ptr != nullptr) { /* if already started, the still command succeeded */ @@ -1249,18 +1249,18 @@ start(enum Rotation rotation) } /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(path_accel, O_RDONLY); + DevMgr::getHandle(path_accel, h); - if (fd < 0) { + if (!h.isValid()) { goto fail; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - px4_close(fd); + if (h.ioctl(SENSORIOCSPOLLRATE, (void *)SENSOR_POLLRATE_DEFAULT) < 0) { + DevMgr::releaseHandle(h); goto fail; } - px4_close(fd); + DevMgr::releaseHandle(h); return 0; fail: @@ -1469,7 +1469,7 @@ gyrosim_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; - const char *myoptarg = NULL; + const char *myoptarg = nullptr; while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) {