From b7a63bfdf3a4fdc9045c0748a09d2cfaa5d8a3be Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 7 Jan 2020 13:20:54 -0500 Subject: [PATCH] drivers/adc: move to ModuleBase and split out header --- src/drivers/adc/{adc.cpp => ADC.cpp} | 167 ++++++++++----------------- src/drivers/adc/ADC.hpp | 110 ++++++++++++++++++ src/drivers/adc/CMakeLists.txt | 6 +- 3 files changed, 175 insertions(+), 108 deletions(-) rename src/drivers/adc/{adc.cpp => ADC.cpp} (75%) create mode 100644 src/drivers/adc/ADC.hpp diff --git a/src/drivers/adc/adc.cpp b/src/drivers/adc/ADC.cpp similarity index 75% rename from src/drivers/adc/adc.cpp rename to src/drivers/adc/ADC.cpp index 6024648345..d4abd96580 100644 --- a/src/drivers/adc/adc.cpp +++ b/src/drivers/adc/ADC.cpp @@ -31,71 +31,7 @@ * ****************************************************************************/ -/** - * @file adc.cpp - * - * Driver for an ADC. - * - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -using namespace time_literals; - -#ifndef ADC_CHANNELS -#error "board needs to define ADC_CHANNELS to use this driver" -#endif - -#define ADC_TOTAL_CHANNELS 32 - - -class ADC : public cdev::CDev, public px4::ScheduledWorkItem -{ -public: - ADC(uint32_t base_address, uint32_t channels); - ~ADC() override; - - int init() override; - - ssize_t read(cdev::file_t *filp, char *buffer, size_t len) override; - -private: - void Run() override; - - /** - * Sample a single channel and return the measured value. - * - * @param channel The channel to sample. - * @return The sampled value, or UINT32_MAX if sampling failed. - */ - uint32_t sample(unsigned channel); - - void update_adc_report(hrt_abstime now); - void update_system_power(hrt_abstime now); - - - static const hrt_abstime kINTERVAL{10_ms}; /**< 100Hz base rate */ - - perf_counter_t _sample_perf; - - unsigned _channel_count{0}; - const uint32_t _base_address; - px4_adc_msg_t *_samples{nullptr}; /**< sample buffer */ - - uORB::Publication _to_adc_report{ORB_ID(adc_report)}; - uORB::Publication _to_system_power{ORB_ID(system_power)}; -}; +#include "ADC.hpp" ADC::ADC(uint32_t base_address, uint32_t channels) : CDev(ADC0_DEVICE_PATH), @@ -145,8 +81,7 @@ ADC::~ADC() px4_arch_adc_uninit(_base_address); } -int -ADC::init() +int ADC::init() { int ret_init = px4_arch_adc_init(_base_address); @@ -169,8 +104,7 @@ ADC::init() return PX4_OK; } -ssize_t -ADC::read(cdev::file_t *filp, char *buffer, size_t len) +ssize_t ADC::read(cdev::file_t *filp, char *buffer, size_t len) { const size_t maxsize = sizeof(px4_adc_msg_t) * _channel_count; @@ -186,8 +120,7 @@ ADC::read(cdev::file_t *filp, char *buffer, size_t len) return len; } -void -ADC::Run() +void ADC::Run() { lock(); hrt_abstime now = hrt_absolute_time(); @@ -202,8 +135,7 @@ ADC::Run() unlock(); } -void -ADC::update_adc_report(hrt_abstime now) +void ADC::update_adc_report(hrt_abstime now) { adc_report_s adc = {}; adc.timestamp = now; @@ -222,8 +154,7 @@ ADC::update_adc_report(hrt_abstime now) _to_adc_report.publish(adc); } -void -ADC::update_system_power(hrt_abstime now) +void ADC::update_system_power(hrt_abstime now) { #if defined (BOARD_ADC_USB_CONNECTED) system_power_s system_power {}; @@ -306,8 +237,7 @@ ADC::update_system_power(hrt_abstime now) #endif // BOARD_ADC_USB_CONNECTED } -uint32_t -ADC::sample(unsigned channel) +uint32_t ADC::sample(unsigned channel) { perf_begin(_sample_perf); uint32_t result = px4_arch_adc_sample(_base_address, channel); @@ -320,19 +250,8 @@ ADC::sample(unsigned channel) return result; } -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int adc_main(int argc, char *argv[]); - -namespace +int ADC::test() { -ADC *g_adc{nullptr}; - -int -test(void) -{ - int fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY); if (fd < 0) { @@ -356,38 +275,74 @@ test(void) } printf("\n"); - px4_usleep(500000); + px4_usleep(100000); } px4_close(fd); return 0; } + +int ADC::custom_command(int argc, char *argv[]) +{ + const char *verb = argv[0]; + + if (!strcmp(verb, "test")) { + if (is_running()) { + return _object.load()->test(); + } + + return PX4_ERROR; + } + + return print_usage("unknown command"); } -int -adc_main(int argc, char *argv[]) +int ADC::task_spawn(int argc, char *argv[]) { - if (g_adc == nullptr) { - g_adc = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS); + ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS); - if (g_adc == nullptr) { - PX4_ERR("couldn't allocate the ADC driver"); - return 1; + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() == PX4_OK) { + return PX4_OK; } - if (g_adc->init() != OK) { - delete g_adc; - PX4_ERR("ADC init failed"); - return 1; - } + } else { + PX4_ERR("alloc failed"); } - if (argc > 1) { - if (!strcmp(argv[1], "test")) { - return test(); - } + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int ADC::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); } + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +ADC driver. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("adc", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND("test"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; } + +extern "C" __EXPORT int adc_main(int argc, char *argv[]) +{ + return ADC::main(argc, argv); +} diff --git a/src/drivers/adc/ADC.hpp b/src/drivers/adc/ADC.hpp new file mode 100644 index 0000000000..380d0f5c59 --- /dev/null +++ b/src/drivers/adc/ADC.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file adc.cpp + * + * Driver for an ADC. + * + */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +#ifndef ADC_CHANNELS +#error "board needs to define ADC_CHANNELS to use this driver" +#endif + +#define ADC_TOTAL_CHANNELS 32 + +class ADC : public ModuleBase, public cdev::CDev, public px4::ScheduledWorkItem +{ +public: + ADC(uint32_t base_address = SYSTEM_ADC_BASE, uint32_t channels = ADC_CHANNELS); + + ~ADC() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + int init() override; + + int test(); + +private: + + void Run() override; + + ssize_t read(cdev::file_t *filp, char *buffer, size_t len) override; + + /** + * Sample a single channel and return the measured value. + * + * @param channel The channel to sample. + * @return The sampled value, or UINT32_MAX if sampling failed. + */ + uint32_t sample(unsigned channel); + + void update_adc_report(hrt_abstime now); + void update_system_power(hrt_abstime now); + + static const hrt_abstime kINTERVAL{10_ms}; /**< 100Hz base rate */ + + perf_counter_t _sample_perf; + + unsigned _channel_count{0}; + const uint32_t _base_address; + px4_adc_msg_t *_samples{nullptr}; /**< sample buffer */ + + uORB::Publication _to_adc_report{ORB_ID(adc_report)}; + uORB::Publication _to_system_power{ORB_ID(system_power)}; +}; diff --git a/src/drivers/adc/CMakeLists.txt b/src/drivers/adc/CMakeLists.txt index 5e52fbf6df..f8a535113a 100644 --- a/src/drivers/adc/CMakeLists.txt +++ b/src/drivers/adc/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,7 +35,9 @@ px4_add_module( MODULE drivers__adc MAIN adc SRCS - adc.cpp + ADC.cpp + ADC.hpp DEPENDS arch_adc + px4_work_queue )