From 746f13d2d402932ef9c179acd38d8d2e5ff192fd Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 5 Aug 2019 21:06:56 -0400 Subject: [PATCH] stm32/adc move to new WQ --- src/drivers/stm32/adc/adc.cpp | 158 ++++++++++++++-------------------- 1 file changed, 67 insertions(+), 91 deletions(-) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index f44599882927..d31cf2889b67 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-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 @@ -40,35 +40,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 + +using namespace time_literals; #if defined(ADC_CHANNELS) +#define ADC_TOTAL_CHANNELS 32 + /* * Register accessors. * For now, no reason not to just use ADC1. @@ -118,7 +109,7 @@ # endif #endif -class ADC : public cdev::CDev +class ADC : public cdev::CDev, public px4::ScheduledWorkItem { public: ADC(uint32_t base_address, uint32_t channels); @@ -134,22 +125,7 @@ class ADC : public cdev::CDev 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; - - unsigned _channel_count; - uint32_t _base_address; - px4_adc_msg_t *_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. @@ -158,28 +134,35 @@ class ADC : public cdev::CDev * @return The sampled value, or 0xffff if * sampling failed. */ - uint16_t _sample(unsigned channel); + uint16_t sample(unsigned channel); + + void update_adc_report(hrt_abstime now); + void update_system_power(hrt_abstime now); - // update system_power ORB topic, only on FMUv2 - 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}; + 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)}; }; ADC::ADC(uint32_t base_address, uint32_t channels) : CDev(ADC0_DEVICE_PATH), + ScheduledWorkItem(px4::wq_configurations::hp_default), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), - _channel_count(0), - _base_address(base_address), - _samples(nullptr), - _to_system_power(nullptr), - _to_adc_report(nullptr) + _base_address(base_address) { /* always enable the temperature sensor */ channels |= 1 << 16; /* allocate the sample array */ - for (unsigned i = 0; i < 32; i++) { + for (unsigned i = 0; i < ADC_TOTAL_CHANNELS; i++) { if (channels & (1 << i)) { _channel_count++; } @@ -195,7 +178,7 @@ ADC::ADC(uint32_t base_address, uint32_t channels) : if (_samples != nullptr) { unsigned index = 0; - for (unsigned i = 0; i < 32; i++) { + for (unsigned i = 0; i < ADC_TOTAL_CHANNELS; i++) { if (channels & (1 << i)) { _samples[index].am_channel = i; _samples[index].am_data = 0; @@ -210,6 +193,8 @@ ADC::~ADC() if (_samples != nullptr) { delete _samples; } + + perf_free(_sample_perf); } int board_adc_init(uint32_t base_address) @@ -349,10 +334,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; } @@ -360,24 +345,19 @@ ADC::open_first(struct file *filp) int ADC::close_last(struct file *filp) { - hrt_cancel(&_call); - return 0; -} + ScheduleClear(); -void -ADC::_tick_trampoline(void *arg) -{ - (reinterpret_cast(arg))->_tick(); + return 0; } 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); @@ -401,21 +381,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; - system_power.voltage3v3_v = 0; - system_power.v3v3_valid = 0; - /* Assume HW provides only ADC_SCALED_V5_SENSE */ int cnt = 1; /* HW provides both ADC_SCALED_V5_SENSE and ADC_SCALED_V3V3_SENSORS_SENSE */ @@ -481,30 +456,23 @@ ADC::update_system_power(hrt_abstime now) #ifdef BOARD_ADC_PERIPH_5V_OC // OC pins are active low system_power.periph_5v_oc = BOARD_ADC_PERIPH_5V_OC; -#else - system_power.periph_5v_oc = 0; #endif + #ifdef BOARD_ADC_HIPOWER_5V_OC system_power.hipower_5v_oc = BOARD_ADC_HIPOWER_5V_OC; -#else - system_power.hipower_5v_oc = 0; #endif /* 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 board_adc_sample(uint32_t base_address, unsigned channel) { - /* clear any previous EOC */ + irqstate_t flags = px4_enter_critical_section(); + /* clear any previous EOC */ if (rSR(base_address) & ADC_SR_EOC) { rSR(base_address) &= ~ADC_SR_EOC; } @@ -514,23 +482,27 @@ uint16_t board_adc_sample(uint32_t base_address, unsigned channel) rCR2(base_address) |= ADC_CR2_SWSTART; /* wait for the conversion to complete */ - hrt_abstime now = hrt_absolute_time(); + const hrt_abstime now = hrt_absolute_time(); while (!(rSR(base_address) & ADC_SR_EOC)) { /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ if ((hrt_absolute_time() - now) > 50) { + px4_leave_critical_section(flags); return 0xffff; } } /* read the result and clear EOC */ uint16_t result = rDR(base_address); + + px4_leave_critical_section(flags); + return result; } uint16_t -ADC::_sample(unsigned channel) +ADC::sample(unsigned channel) { perf_begin(_sample_perf); uint16_t result = board_adc_sample(_base_address, channel); @@ -552,22 +524,24 @@ namespace { ADC *g_adc; -void +int test(void) { int fd = open(ADC0_DEVICE_PATH, O_RDONLY); if (fd < 0) { - err(1, "can't open ADC device"); + PX4_ERR("can't open ADC device %d", errno); + return 1; } for (unsigned i = 0; i < 50; i++) { - px4_adc_msg_t data[PX4_MAX_ADC_CHANNELS]; + px4_adc_msg_t data[ADC_TOTAL_CHANNELS]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) { - errx(1, "read error"); + PX4_ERR("read error"); + return 1; } unsigned channels = count / sizeof(data[0]); @@ -580,7 +554,7 @@ test(void) px4_usleep(500000); } - exit(0); + return 0; } } @@ -592,21 +566,23 @@ adc_main(int argc, char *argv[]) g_adc = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS); if (g_adc == nullptr) { - errx(1, "couldn't allocate the ADC driver"); + PX4_ERR("couldn't allocate the ADC driver"); + return 1; } if (g_adc->init() != OK) { delete g_adc; - errx(1, "ADC init failed"); + PX4_ERR("ADC init failed"); + return 1; } } if (argc > 1) { if (!strcmp(argv[1], "test")) { - test(); + return test(); } } - exit(0); + return 0; } #endif