From 7ddd5712c53da99715ff14602510f44fe56fe8b6 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sun, 1 Nov 2015 23:43:46 -0800 Subject: [PATCH] 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 | 124 +++++------------- 4 files changed, 41 insertions(+), 88 deletions(-) create mode 160000 src/lib/DriverFramework diff --git a/.gitmodules b/.gitmodules index 744dcd2787..60cdcb1105 100644 --- a/.gitmodules +++ b/.gitmodules @@ -28,3 +28,6 @@ [submodule "src/lib/matrix"] path = src/lib/matrix url = https://github.com/PX4/Matrix.git +[submodule "src/lib/DriverFramework"] + path = src/lib/DriverFramework + url = https://github.com/PX4/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 c72cc1a494..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 ~ADCSIM(); - virtual int init(); - - 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,13 +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), - _call(), + VirtDriverObj("adcsim", ADC_BASE_DEV_PATH, 10000), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr) @@ -151,75 +146,34 @@ ADCSIM::~ADCSIM() } } -int -ADCSIM::init() +int ADCSIM::read(DriverHandle &h, adc_msg_s *sample, size_t num_samples) { - DEVICE_DEBUG("init done"); + 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); - /* create the device node */ - return VDev::init(); -} - -int -ADCSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - 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; - - 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; + return -1; } void -ADCSIM::_tick_trampoline(void *arg) -{ - (reinterpret_cast(arg))->_tick(); -} - -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 @@ -246,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; } @@ -271,7 +225,7 @@ test(void) usleep(500000); } - px4_close(fd); + DriverMgr::releaseHandle(h); return 0; } } @@ -289,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) {