PX4 Firmware
PX4 Autopilot Software http://px4.io
adc.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file adc.cpp
36  *
37  * Driver for an ADC.
38  *
39  */
40 #include <stdint.h>
41 #include <drivers/drv_adc.h>
42 #include <drivers/drv_hrt.h>
43 #include <lib/cdev/CDev.hpp>
44 #include <lib/perf/perf_counter.h>
45 #include <px4_arch/adc.h>
46 #include <px4_platform_common/px4_config.h>
47 #include <px4_platform_common/log.h>
48 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
49 #include <uORB/Publication.hpp>
50 #include <uORB/topics/adc_report.h>
52 
53 
54 using namespace time_literals;
55 
56 #ifndef ADC_CHANNELS
57 #error "board needs to define ADC_CHANNELS to use this driver"
58 #endif
59 
60 #define ADC_TOTAL_CHANNELS 32
61 
62 
63 class ADC : public cdev::CDev, public px4::ScheduledWorkItem
64 {
65 public:
66  ADC(uint32_t base_address, uint32_t channels);
67  ~ADC();
68 
69  virtual int init();
70 
71  virtual int ioctl(file *filp, int cmd, unsigned long arg);
72  virtual ssize_t read(file *filp, char *buffer, size_t len);
73 
74 protected:
75  virtual int open_first(struct file *filp);
76  virtual int close_last(struct file *filp);
77 
78 private:
79  void Run() override;
80 
81  /**
82  * Sample a single channel and return the measured value.
83  *
84  * @param channel The channel to sample.
85  * @return The sampled value, or UINT32_MAX if sampling failed.
86  */
87  uint32_t sample(unsigned channel);
88 
89  void update_adc_report(hrt_abstime now);
90  void update_system_power(hrt_abstime now);
91 
92 
93  static const hrt_abstime kINTERVAL{10_ms}; /**< 100Hz base rate */
94 
96 
97  unsigned _channel_count{0};
98  const uint32_t _base_address;
99  px4_adc_msg_t *_samples{nullptr}; /**< sample buffer */
100 
101  uORB::Publication<adc_report_s> _to_adc_report{ORB_ID(adc_report)};
102  uORB::Publication<system_power_s> _to_system_power{ORB_ID(system_power)};
103 };
104 
105 ADC::ADC(uint32_t base_address, uint32_t channels) :
106  CDev(ADC0_DEVICE_PATH),
107  ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
108  _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")),
109  _base_address(base_address)
110 {
111  /* always enable the temperature sensor */
112  channels |= px4_arch_adc_temp_sensor_mask();
113 
114  /* allocate the sample array */
115  for (unsigned i = 0; i < ADC_TOTAL_CHANNELS; i++) {
116  if (channels & (1 << i)) {
117  _channel_count++;
118  }
119  }
120 
122  PX4_ERR("PX4_MAX_ADC_CHANNELS is too small:is %d needed:%d", PX4_MAX_ADC_CHANNELS, _channel_count);
123  }
124 
126 
127  /* prefill the channel numbers in the sample array */
128  if (_samples != nullptr) {
129  unsigned index = 0;
130 
131  for (unsigned i = 0; i < ADC_TOTAL_CHANNELS; i++) {
132  if (channels & (1 << i)) {
133  _samples[index].am_channel = i;
134  _samples[index].am_data = 0;
135  index++;
136  }
137  }
138  }
139 }
140 
142 {
143  if (_samples != nullptr) {
144  delete _samples;
145  }
146 
149 }
150 
151 int
153 {
155 
156  if (rv < 0) {
157  PX4_DEBUG("sample timeout");
158  return rv;
159  }
160 
161  /* create the device node */
162  return CDev::init();
163 }
164 
165 int
166 ADC::ioctl(file *filp, int cmd, unsigned long arg)
167 {
168  return -ENOTTY;
169 }
170 
171 ssize_t
172 ADC::read(file *filp, char *buffer, size_t len)
173 {
174  const size_t maxsize = sizeof(px4_adc_msg_t) * _channel_count;
175 
176  if (len > maxsize) {
177  len = maxsize;
178  }
179 
180  /* block interrupts while copying samples to avoid racing with an update */
181  irqstate_t flags = px4_enter_critical_section();
182  memcpy(buffer, _samples, len);
183  px4_leave_critical_section(flags);
184 
185  return len;
186 }
187 
188 int
189 ADC::open_first(struct file *filp)
190 {
191  /* get fresh data */
192  Run();
193 
194  /* and schedule regular updates */
195  ScheduleOnInterval(kINTERVAL, kINTERVAL);
196 
197  return 0;
198 }
199 
200 int
201 ADC::close_last(struct file *filp)
202 {
203  ScheduleClear();
204 
205  return 0;
206 }
207 
208 void
210 {
212 
213  /* scan the channel set and sample each */
214  for (unsigned i = 0; i < _channel_count; i++) {
215  _samples[i].am_data = sample(_samples[i].am_channel);
216  }
217 
218  update_adc_report(now);
219  update_system_power(now);
220 }
221 
222 void
224 {
225  adc_report_s adc = {};
226  adc.timestamp = now;
227 
228  unsigned max_num = _channel_count;
229 
230  if (max_num > (sizeof(adc.channel_id) / sizeof(adc.channel_id[0]))) {
231  max_num = (sizeof(adc.channel_id) / sizeof(adc.channel_id[0]));
232  }
233 
234  for (unsigned i = 0; i < max_num; i++) {
235  adc.channel_id[i] = _samples[i].am_channel;
236  adc.channel_value[i] = _samples[i].am_data * 3.3f / px4_arch_adc_dn_fullcount();
237  ;
238  }
239 
240  _to_adc_report.publish(adc);
241 }
242 
243 void
245 {
246 #if defined (BOARD_ADC_USB_CONNECTED)
247  system_power_s system_power {};
248  system_power.timestamp = now;
249 
250  /* Assume HW provides only ADC_SCALED_V5_SENSE */
251  int cnt = 1;
252  /* HW provides both ADC_SCALED_V5_SENSE and ADC_SCALED_V3V3_SENSORS_SENSE */
253 # if defined(ADC_SCALED_V5_SENSE) && defined(ADC_SCALED_V3V3_SENSORS_SENSE)
254  cnt++;
255 # endif
256 
257  for (unsigned i = 0; i < _channel_count; i++) {
258 # if defined(ADC_SCALED_V5_SENSE)
259 
260  if (_samples[i].am_channel == ADC_SCALED_V5_SENSE) {
261  // it is 2:1 scaled
262  system_power.voltage5v_v = _samples[i].am_data * (ADC_V5_V_FULL_SCALE / px4_arch_adc_dn_fullcount());
263  cnt--;
264 
265  } else
266 # endif
267 # if defined(ADC_SCALED_V3V3_SENSORS_SENSE)
268  {
269  if (_samples[i].am_channel == ADC_SCALED_V3V3_SENSORS_SENSE) {
270  // it is 2:1 scaled
271  system_power.voltage3v3_v = _samples[i].am_data * (ADC_3V3_SCALE * (3.3f / px4_arch_adc_dn_fullcount()));
272  system_power.v3v3_valid = 1;
273  cnt--;
274  }
275  }
276 
277 # endif
278 
279  if (cnt == 0) {
280  break;
281  }
282  }
283 
284  /* Note once the board_config.h provides BOARD_ADC_USB_CONNECTED,
285  * It must provide the true logic GPIO BOARD_ADC_xxxx macros.
286  */
287  // these are not ADC related, but it is convenient to
288  // publish these to the same topic
289 
290  system_power.usb_connected = BOARD_ADC_USB_CONNECTED;
291  /* If provided used the Valid signal from HW*/
292 #if defined(BOARD_ADC_USB_VALID)
293  system_power.usb_valid = BOARD_ADC_USB_VALID;
294 #else
295  /* If not provided then use connected */
296  system_power.usb_valid = system_power.usb_connected;
297 #endif
298 
299  /* The valid signals (HW dependent) are associated with each brick */
300 #if !defined(BOARD_NUMBER_DIGITAL_BRICKS)
301  bool valid_chan[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
302  system_power.brick_valid = 0;
303 
304  for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
305  system_power.brick_valid |= valid_chan[b] ? 1 << b : 0;
306  }
307 
308 #endif
309 
310  system_power.servo_valid = BOARD_ADC_SERVO_VALID;
311 
312 #ifdef BOARD_ADC_PERIPH_5V_OC
313  // OC pins are active low
314  system_power.periph_5v_oc = BOARD_ADC_PERIPH_5V_OC;
315 #endif
316 
317 #ifdef BOARD_ADC_HIPOWER_5V_OC
318  system_power.hipower_5v_oc = BOARD_ADC_HIPOWER_5V_OC;
319 #endif
320 
321  /* lazily publish */
322  _to_system_power.publish(system_power);
323 
324 #endif // BOARD_ADC_USB_CONNECTED
325 }
326 
327 uint32_t
328 ADC::sample(unsigned channel)
329 {
331  uint32_t result = px4_arch_adc_sample(_base_address, channel);
332 
333  if (result == UINT32_MAX) {
334  PX4_ERR("sample timeout");
335  }
336 
338  return result;
339 }
340 
341 /*
342  * Driver 'main' command.
343  */
344 extern "C" __EXPORT int adc_main(int argc, char *argv[]);
345 
346 namespace
347 {
348 ADC *g_adc{nullptr};
349 
350 int
351 test(void)
352 {
353 
354  int fd = open(ADC0_DEVICE_PATH, O_RDONLY);
355 
356  if (fd < 0) {
357  PX4_ERR("can't open ADC device %d", errno);
358  return 1;
359  }
360 
361  for (unsigned i = 0; i < 20; i++) {
363  ssize_t count = read(fd, data, sizeof(data));
364 
365  if (count < 0) {
366  PX4_ERR("read error");
367  return 1;
368  }
369 
370  unsigned channels = count / sizeof(data[0]);
371 
372  for (unsigned j = 0; j < channels; j++) {
373  printf("%d: %u ", data[j].am_channel, data[j].am_data);
374  }
375 
376  printf("\n");
377  px4_usleep(500000);
378  }
379 
380  return 0;
381 }
382 }
383 
384 int
385 adc_main(int argc, char *argv[])
386 {
387  if (g_adc == nullptr) {
388  g_adc = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS);
389 
390  if (g_adc == nullptr) {
391  PX4_ERR("couldn't allocate the ADC driver");
392  return 1;
393  }
394 
395  if (g_adc->init() != OK) {
396  delete g_adc;
397  PX4_ERR("ADC init failed");
398  return 1;
399  }
400  }
401 
402  if (argc > 1) {
403  if (!strcmp(argv[1], "test")) {
404  return test();
405  }
406  }
407 
408  return 0;
409 }
px4_adc_msg_t
Definition: drv_adc.h:56
virtual ssize_t read(file *filp, char *buffer, size_t len)
Definition: adc.cpp:172
uORB::Publication< system_power_s > _to_system_power
Definition: adc.cpp:102
virtual int open(file_t *filep)
Handle an open of the device.
Definition: CDev.cpp:141
float channel_value[12]
Definition: adc_report.h:54
measure the time elapsed performing an event
Definition: perf_counter.h:56
void Run() override
Definition: adc.cpp:209
uint64_t timestamp
Definition: adc_report.h:53
void px4_arch_adc_uninit(uint32_t base_address)
Uninitialize ADC hardware.
#define PX4_MAX_ADC_CHANNELS
Definition: drv_adc.h:52
static const hrt_abstime kINTERVAL
100Hz base rate
Definition: adc.cpp:93
unsigned _channel_count
Definition: adc.cpp:97
Definition: I2C.hpp:51
uint32_t px4_arch_adc_dn_fullcount(void)
Get the adc digital number full count.
Definition: adc.cpp:63
ADC driver interface.
#define ADC0_DEVICE_PATH
Definition: drv_adc.h:59
High-resolution timer with callouts and timekeeping.
void update_system_power(hrt_abstime now)
Definition: adc.cpp:244
uint64_t timestamp
Definition: system_power.h:61
px4_adc_msg_t * _samples
sample buffer
Definition: adc.cpp:99
#define ADC_TOTAL_CHANNELS
Definition: adc.cpp:60
static void read(bootloader_app_shared_t *pshared)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel)
Read a sample from the ADC.
uORB::Publication< adc_report_s > _to_adc_report
Definition: adc.cpp:101
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
Abstract class for any character device.
Definition: CDev.hpp:58
virtual int init()
Definition: adc.cpp:152
Header common to all counters.
virtual int open_first(struct file *filp)
Definition: adc.cpp:189
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
~ADC()
Definition: adc.cpp:141
__BEGIN_DECLS int px4_arch_adc_init(uint32_t base_address)
Initialize ADC hardware.
#define perf_alloc(a, b)
Definition: px4io.h:59
__EXPORT int adc_main(int argc, char *argv[])
Definition: adc.cpp:385
uint8_t * data
Definition: dataman.cpp:149
void perf_end(perf_counter_t handle)
End a performance event.
void test(enum LPS25H_BUS busid)
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
Definition: lps25h.cpp:792
void update_adc_report(hrt_abstime now)
Definition: adc.cpp:223
ADC(uint32_t base_address, uint32_t channels)
Definition: adc.cpp:105
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
int fd
Definition: dataman.cpp:146
int16_t channel_id[12]
Definition: adc_report.h:55
uint32_t sample(unsigned channel)
Sample a single channel and return the measured value.
Definition: adc.cpp:328
struct @83::@85::@87 file
Definition: bst.cpp:62
virtual int ioctl(file *filp, int cmd, unsigned long arg)
Definition: adc.cpp:166
#define OK
Definition: uavcan_main.cpp:71
perf_counter_t _sample_perf
Definition: adc.cpp:95
uint32_t px4_arch_adc_temp_sensor_mask(void)
Get the temperature sensor channel bitmask.
const uint32_t _base_address
Definition: adc.cpp:98
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
virtual int close_last(struct file *filp)
Definition: adc.cpp:201
Performance measuring tools.