diff --git a/src/drivers/kinetis/adc/adc.cpp b/src/drivers/kinetis/adc/adc.cpp index e1888d4318..50f307c3c0 100644 --- a/src/drivers/kinetis/adc/adc.cpp +++ b/src/drivers/kinetis/adc/adc.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2016-2019 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 @@ -43,38 +43,26 @@ * and so forth. It avoids the gross complexity of the NuttX ADC driver. */ -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - #include #include #include #include -#include - -#include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include + +using namespace time_literals; #if defined(ADC_CHANNELS) -typedef uint32_t adc_chan_t; #define ADC_TOTAL_CHANNELS 32 #define _REG(_addr) (*(volatile uint32_t *)(_addr)) @@ -111,10 +99,10 @@ typedef uint32_t adc_chan_t; #define rCLM1(adc) REG(adc, KINETIS_ADC_CLM1_OFFSET) /* ADC minus-side general calibration value register */ #define rCLM0(adc) REG(adc, KINETIS_ADC_CLM0_OFFSET) /* ADC minus-side general calibration value register */ -class ADC : public device::CDev +class ADC : public cdev::CDev, public px4::ScheduledWorkItem { public: - ADC(adc_chan_t channels); + ADC(uint32_t channels); ~ADC(); virtual int init(); @@ -127,23 +115,7 @@ protected: virtual int close_last(struct file *filp); private: - static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - - hrt_call _call; - perf_counter_t _sample_perf; - - adc_chan_t _channels; /**< bits set for channels */ - unsigned _channel_count; - adc_msg_s *_samples; /**< sample buffer */ - - orb_advert_t _to_system_power; - orb_advert_t _to_adc_report; - - /** work trampoline */ - static void _tick_trampoline(void *arg); - - /** worker function */ - void _tick(); + void Run() override; /** * Sample a single channel and return the measured value. @@ -152,25 +124,28 @@ private: * @return The sampled value, or 0xffff if * sampling failed. */ - uint16_t _sample(unsigned channel); + uint16_t sample(unsigned channel); - // update system_power ORB topic, only on FMUv2 - void update_system_power(hrt_abstime now); + void update_adc_report(hrt_abstime now); + void update_system_power(hrt_abstime now); - void update_adc_report(hrt_abstime now); + + static const hrt_abstime kINTERVAL{10_ms}; /**< 100Hz base rate */ + + perf_counter_t _sample_perf; + + unsigned _channel_count{0}; + 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)}; }; -ADC::ADC(adc_chan_t channels) : - CDev("adc", ADC0_DEVICE_PATH), - _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), - _channels(channels), - _channel_count(0), - _samples(nullptr), - _to_system_power(nullptr), - _to_adc_report(nullptr) +ADC::ADC(uint32_t channels) : + CDev(ADC0_DEVICE_PATH), + ScheduledWorkItem(px4::wq_configurations::hp_default), + _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")) { - _debug_enabled = true; - /* always enable the temperature sensor */ channels |= 1 << (ADC_SC1_ADCH_TEMP >> ADC_SC1_ADCH_SHIFT); @@ -181,10 +156,13 @@ ADC::ADC(adc_chan_t channels) : } } - _samples = new adc_msg_s[_channel_count]; + if (_channel_count > PX4_MAX_ADC_CHANNELS) { + PX4_ERR("PX4_MAX_ADC_CHANNELS is too small:is %d needed:%d", PX4_MAX_ADC_CHANNELS, _channel_count); + } + + _samples = new px4_adc_msg_t[_channel_count]; /* prefill the channel numbers in the sample array */ - if (_samples != nullptr) { unsigned index = 0; @@ -207,10 +185,11 @@ ADC::~ADC() irqstate_t flags = px4_enter_critical_section(); _REG(KINETIS_SIM_SCGC3) &= ~SIM_SCGC3_ADC1; px4_leave_critical_section(flags); + + perf_free(_sample_perf); } -int -ADC::init() +int board_adc_init() { /* Input is Buss Clock 56 Mhz We will use /8 for 7 Mhz */ @@ -266,13 +245,22 @@ ADC::init() /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ if ((hrt_absolute_time() - now) > 500) { - DEVICE_LOG("sample timeout"); return -1; } - - break; } + return OK; +} + +int +ADC::init() +{ + int rv = board_adc_init(); + + if (rv < 0) { + PX4_DEBUG("sample timeout"); + return rv; + } /* create the device node */ return CDev::init(); @@ -287,7 +275,7 @@ ADC::ioctl(file *filp, int cmd, unsigned long arg) ssize_t ADC::read(file *filp, char *buffer, size_t len) { - const size_t maxsize = sizeof(adc_msg_s) * _channel_count; + const size_t maxsize = sizeof(px4_adc_msg_t) * _channel_count; if (len > maxsize) { len = maxsize; @@ -305,10 +293,10 @@ int ADC::open_first(struct file *filp) { /* get fresh data */ - _tick(); + Run(); /* and schedule regular updates */ - hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); + ScheduleOnInterval(kINTERVAL, kINTERVAL); return 0; } @@ -316,24 +304,19 @@ ADC::open_first(struct file *filp) int ADC::close_last(struct file *filp) { - hrt_cancel(&_call); + ScheduleClear(); + return 0; } void -ADC::_tick_trampoline(void *arg) -{ - (reinterpret_cast(arg))->_tick(); -} - -void -ADC::_tick() +ADC::Run() { hrt_abstime now = hrt_absolute_time(); /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) { - _samples[i].am_data = _sample(_samples[i].am_channel); + _samples[i].am_data = sample(_samples[i].am_channel); } update_adc_report(now); @@ -357,19 +340,16 @@ ADC::update_adc_report(hrt_abstime now) adc.channel_value[i] = _samples[i].am_data * 3.3f / 4096.0f; } - int instance; - orb_publish_auto(ORB_ID(adc_report), &_to_adc_report, &adc, &instance, ORB_PRIO_HIGH); + _to_adc_report.publish(adc); } void ADC::update_system_power(hrt_abstime now) { #if defined (BOARD_ADC_USB_CONNECTED) - system_power_s system_power = {}; + system_power_s system_power {}; system_power.timestamp = now; - system_power.voltage5v_v = 0; - #if defined(ADC_5V_RAIL_SENSE) for (unsigned i = 0; i < _channel_count; i++) { @@ -382,7 +362,6 @@ ADC::update_system_power(hrt_abstime now) #endif - /* Note once the board_config.h provides BOARD_ADC_USB_CONNECTED, * It must provide the true logic GPIO BOARD_ADC_xxxx macros. */ @@ -406,42 +385,50 @@ ADC::update_system_power(hrt_abstime now) system_power.hipower_5v_oc = BOARD_ADC_HIPOWER_5V_OC; /* lazily publish */ - if (_to_system_power != nullptr) { - orb_publish(ORB_ID(system_power), _to_system_power, &system_power); - - } else { - _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); - } + _to_system_power.publish(system_power); #endif // BOARD_ADC_USB_CONNECTED } -uint16_t -ADC::_sample(unsigned channel) +uint16_t board_adc_sample(unsigned channel) { - perf_begin(_sample_perf); + irqstate_t flags = px4_enter_critical_section(); /* clear any previous COCC */ - uint16_t result = rRA(1); + rRA(1); /* run a single conversion right now - should take about 35 cycles (5 microseconds) max */ - rSC1A(1) = ADC_SC1_ADCH(channel); /* wait for the conversion to complete */ - hrt_abstime now = hrt_absolute_time(); + const hrt_abstime now = hrt_absolute_time(); while (!(rSC1A(1) & ADC_SC1_COCO)) { /* don't wait for more than 10us, since that means something broke - should reset here if we see this */ if ((hrt_absolute_time() - now) > 10) { - DEVICE_LOG("sample timeout"); + px4_leave_critical_section(flags); return 0xffff; } } /* read the result and clear EOC */ - result = rRA(1); + uint16_t result = rRA(1); + + px4_leave_critical_section(flags); + + return result; +} + +uint16_t +ADC::sample(unsigned channel) +{ + perf_begin(_sample_perf); + uint16_t result = board_adc_sample(channel); + + if (result == 0xffff) { + PX4_ERR("sample timeout"); + } perf_end(_sample_perf); return result; @@ -454,9 +441,9 @@ extern "C" __EXPORT int adc_main(int argc, char *argv[]); namespace { -ADC *g_adc; +ADC *g_adc{nullptr}; -void +int test(void) { @@ -464,16 +451,16 @@ test(void) if (fd < 0) { PX4_ERR("can't open ADC device %d", errno); - exit(1); + return 1; } for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[ADC_TOTAL_CHANNELS]; + px4_adc_msg_t data[ADC_TOTAL_CHANNELS]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) { PX4_ERR("read error"); - exit(1); + return 1; } unsigned channels = count / sizeof(data[0]); @@ -483,10 +470,10 @@ test(void) } printf("\n"); - usleep(500000); + px4_usleep(500000); } - exit(0); + return 0; } } @@ -499,22 +486,22 @@ adc_main(int argc, char *argv[]) if (g_adc == nullptr) { PX4_ERR("couldn't allocate the ADC driver"); - exit(1); + return 1; } if (g_adc->init() != OK) { delete g_adc; PX4_ERR("ADC init failed"); - exit(1); + return 1; } } if (argc > 1) { if (!strcmp(argv[1], "test")) { - test(); + return test(); } } - exit(0); + return 0; } #endif diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index d31cf2889b..da5836f04d 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -522,7 +522,7 @@ extern "C" __EXPORT int adc_main(int argc, char *argv[]); namespace { -ADC *g_adc; +ADC *g_adc{nullptr}; int test(void)