kinetis/adc move to new WQ

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 <px4_config.h>
-#include <px4_log.h>
-#include <board_config.h>
-#include <drivers/device/device.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <string.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <stdio.h>
-#include <unistd.h>
-
-#include <arch/board/board.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_adc.h>
-
 #include <nuttx/analog/adc.h>
 #include <kinetis.h>
 #include <chip/kinetis_sim.h>
 #include <chip/kinetis_adc.h>
 
-#include <perf/perf_counter.h>
+#include <drivers/drv_adc.h>
-
+#include <drivers/drv_hrt.h>
-#include <uORB/topics/system_power.h>
+#include <lib/cdev/CDev.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_config.h>
+#include <px4_log.h>
+#include <px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/adc_report.h>
+#include <uORB/topics/system_power.h>
+
+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 */
+	void			Run() override;
-
-	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();
 
 	/**
 	 * 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);
+
+	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;
 
-	// update system_power ORB topic, only on FMUv2
+	unsigned			_channel_count{0};
-	void update_system_power(hrt_abstime now);
+	px4_adc_msg_t			*_samples{nullptr};	/**< sample buffer */
 
-	void update_adc_report(hrt_abstime now);
+	uORB::Publication<adc_report_s>		_to_adc_report{ORB_ID(adc_report)};
+	uORB::Publication<system_power_s>	_to_system_power{ORB_ID(system_power)};
 };
 
-ADC::ADC(adc_chan_t channels) :
+ADC::ADC(uint32_t channels) :
-	CDev("adc", ADC0_DEVICE_PATH),
+	CDev(ADC0_DEVICE_PATH),
-	_sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")),
+	ScheduledWorkItem(px4::wq_configurations::hp_default),
-	_channels(channels),
+	_sample_perf(perf_alloc(PC_ELAPSED, "adc_samples"))
-	_channel_count(0),
-	_samples(nullptr),
-	_to_system_power(nullptr),
-	_to_adc_report(nullptr)
 {
-	_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
+int board_adc_init()
-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
+	return 0;
-ADC::_tick_trampoline(void *arg)
-{
-	(reinterpret_cast<ADC *>(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;
+	_to_adc_report.publish(adc);
-	orb_publish_auto(ORB_ID(adc_report), &_to_adc_report, &adc, &instance, ORB_PRIO_HIGH);
 }
 
 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) {
+	_to_system_power.publish(system_power);
-		orb_publish(ORB_ID(system_power), _to_system_power, &system_power);
-
-	} else {
-		_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
-	}
 
 #endif // BOARD_ADC_USB_CONNECTED
 }
 
-uint16_t
+uint16_t board_adc_sample(unsigned channel)
-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

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)