PX4 Firmware
PX4 Autopilot Software http://px4.io
adc.c
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012 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.c
36  *
37  * Simple ADC support for PX4IO on STM32.
38  */
39 #include <px4_platform_common/px4_config.h>
40 #include <stdint.h>
41 
42 #include <nuttx/arch.h>
43 #include <arch/stm32/chip.h>
44 #include <stm32.h>
45 
46 #include <drivers/drv_hrt.h>
47 #include <perf/perf_counter.h>
48 
49 #define DEBUG
50 #include "px4io.h"
51 
52 /*
53  * Register accessors.
54  * For now, no reason not to just use ADC1.
55  */
56 #define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
57 
58 #define rSR REG(STM32_ADC_SR_OFFSET)
59 #define rCR1 REG(STM32_ADC_CR1_OFFSET)
60 #define rCR2 REG(STM32_ADC_CR2_OFFSET)
61 #define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET)
62 #define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET)
63 #define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET)
64 #define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET)
65 #define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET)
66 #define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET)
67 #define rHTR REG(STM32_ADC_HTR_OFFSET)
68 #define rLTR REG(STM32_ADC_LTR_OFFSET)
69 #define rSQR1 REG(STM32_ADC_SQR1_OFFSET)
70 #define rSQR2 REG(STM32_ADC_SQR2_OFFSET)
71 #define rSQR3 REG(STM32_ADC_SQR3_OFFSET)
72 #define rJSQR REG(STM32_ADC_JSQR_OFFSET)
73 #define rJDR1 REG(STM32_ADC_JDR1_OFFSET)
74 #define rJDR2 REG(STM32_ADC_JDR2_OFFSET)
75 #define rJDR3 REG(STM32_ADC_JDR3_OFFSET)
76 #define rJDR4 REG(STM32_ADC_JDR4_OFFSET)
77 #define rDR REG(STM32_ADC_DR_OFFSET)
78 
80 
81 int
82 adc_init(void)
83 {
84  adc_perf = perf_alloc(PC_ELAPSED, "adc");
85 
86  /* put the ADC into power-down mode */
87  rCR2 &= ~ADC_CR2_ADON;
88  up_udelay(10);
89 
90  /* bring the ADC out of power-down mode */
91  rCR2 |= ADC_CR2_ADON;
92  up_udelay(10);
93 
94  /* do calibration if supported */
95 #ifdef ADC_CR2_CAL
96  rCR2 |= ADC_CR2_RSTCAL;
97  up_udelay(1);
98 
99  if (rCR2 & ADC_CR2_RSTCAL) {
100  return -1;
101  }
102 
103  rCR2 |= ADC_CR2_CAL;
104  up_udelay(100);
105 
106  if (rCR2 & ADC_CR2_CAL) {
107  return -1;
108  }
109 
110 #endif
111 
112  /*
113  * Configure sampling time.
114  *
115  * For electrical protection reasons, we want to be able to have
116  * 10K in series with ADC inputs that leave the board. At 12MHz this
117  * means we need 28.5 cycles of sampling time (per table 43 in the
118  * datasheet).
119  */
120  rSMPR1 = 0b00000000011011011011011011011011;
121  rSMPR2 = 0b00011011011011011011011011011011;
122 
123  rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */
124 
125  /* configure for a single-channel sequence */
126  rSQR1 = 0;
127  rSQR2 = 0;
128  rSQR3 = 0; /* will be updated with the channel at conversion time */
129 
130  return 0;
131 }
132 
133 /*
134  return one measurement, or 0xffff on error
135  */
136 uint16_t
137 adc_measure(unsigned channel)
138 {
139 
140  perf_begin(adc_perf);
141 
142  /* clear any previous EOC */
143  rSR = 0;
144  (void)rDR;
145 
146  /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
147  rSQR3 = channel;
148  rCR2 |= ADC_CR2_ADON;
149 
150  /* wait for the conversion to complete */
152 
153  while (!(rSR & ADC_SR_EOC)) {
154 
155  /* never spin forever - this will give a bogus result though */
156  if (hrt_elapsed_time(&now) > 100) {
157  perf_end(adc_perf);
158  return 0xffff;
159  }
160  }
161 
162  /* read the result and clear EOC */
163  uint16_t result = rDR;
164  rSR = 0;
165 
166  perf_end(adc_perf);
167  return result;
168 }
#define rSQR3
Definition: adc.c:71
measure the time elapsed performing an event
Definition: perf_counter.h:56
#define rSMPR1
Definition: adc.c:61
#define rSQR1
Definition: adc.c:69
High-resolution timer with callouts and timekeeping.
Header common to all counters.
#define perf_alloc(a, b)
Definition: px4io.h:59
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
#define rSMPR2
Definition: adc.c:62
void perf_end(perf_counter_t handle)
End a performance event.
#define rSR
Definition: adc.c:58
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
#define rSQR2
Definition: adc.c:70
#define rCR2
Definition: adc.c:60
General defines and structures for the PX4IO module firmware.
#define rDR
Definition: adc.c:77
int adc_init(void)
Sensors/misc inputs.
Definition: adc.c:82
void perf_begin(perf_counter_t handle)
Begin a performance event.
uint16_t adc_measure(unsigned channel)
Definition: adc.c:137
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
perf_counter_t adc_perf
Definition: adc.c:79
Performance measuring tools.