APM:Libraries
adc.c
Go to the documentation of this file.
1 /*
2 
3 (c) 2017 night_ghost@ykoctpa.ru
4 
5 
6 based on: LeafLabs
7 
8 */
9 
10 #pragma GCC optimize ("O2")
11 
12 #include <hal.h>
13 #include "adc.h"
14 #include <stdbool.h>
15 
16 const adc_dev _adc1 = {
17  .adcx = ADC1,
18 };
20 const adc_dev* const _ADC1 = &_adc1;
21 
22 const adc_dev _adc2 = {
23  .adcx = ADC2,
24 };
26 const adc_dev* const _ADC2 = &_adc2;
27 
28 const adc_dev const _adc3 = {
29  .adcx = ADC3,
30 };
32 const adc_dev* const _ADC3 = &_adc3;
33 
34 __IO uint16_t ADC_ConvertedValue;
35 __IO bool adc_data_ready;
36 
41 void adc_foreach(void (*fn)(const adc_dev*))
42 {
43  fn(_ADC1);
44  fn(_ADC2);
45  fn(_ADC3);
46 }
47 
56 void adc_init(const adc_dev *dev) {
57  /* Enable The HSI */
58  RCC_HSICmd(ENABLE);
59 
60  /* Check that HSI oscillator is ready */
61  while(RCC_GetFlagStatus(RCC_FLAG_HSIRDY) == RESET);
62 
63  if (dev->adcx == ADC1)
64  RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
65  else if (dev->adcx == ADC2)
66  RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC2, ENABLE);
67  else
68  RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC3, ENABLE);
69 
70  ADC_DeInit();
71 
72  ADC_InitTypeDef ADC_InitStructure;
73  ADC_CommonInitTypeDef ADC_CommonInitStructure;
74  /* ADC Common Init **********************************************************/
75  ADC_CommonStructInit(&ADC_CommonInitStructure);
76  ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div4;
77  ADC_CommonInit(&ADC_CommonInitStructure);
78 
79  /* ADCx Init ****************************************************************/
80  ADC_StructInit(&ADC_InitStructure);
81  ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
82  ADC_InitStructure.ADC_ScanConvMode = DISABLE;
83  ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
84  ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
85  ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
86  ADC_InitStructure.ADC_NbrOfConversion = 1;
87  ADC_Init(dev->adcx, &ADC_InitStructure);
88 }
89 
90 #if 0 // unused
91 
99 uint16_t adc_read(const adc_dev *dev, uint8_t channel)
100 {
101  adc_data_ready = false;
102 
103  adc_disable(dev);
104 
105  /* ADC regular channel14 configuration */
107  adc_enable(dev);
108 
109  /* Start ADC Software Conversion */
110  ADC_SoftwareStartConv(dev->adcx);
111 
112  /* Wait until ADC Channel end of conversion */
113  while (ADC_GetFlagStatus(dev->adcx, ADC_FLAG_EOC) == RESET);
114 
115  /* Read ADC conversion result */
116  return ADC_GetConversionValue(dev->adcx);
117 }
118 
119 uint16_t temp_read(void)
120 {
121  uint8_t i;
122  uint16_t res, T_StartupTimeDelay;
123 
124  ADC_TempSensorVrefintCmd(ENABLE);
125  /* Wait until ADC + Temp sensor start */
126  T_StartupTimeDelay = 1024;
127  while (T_StartupTimeDelay--);
128 
129  /* Enable TempSensor and Vrefint channels: channel16 and Channel17 */
131 
132  /* initialize result */
133  res = 0;
134  for(i=4; i>0; i--) {
135  /* start ADC convertion by software */
136  ADC_SoftwareStartConv(ADC1);
137 
138  /* wait until end-of-covertion */
139  while( ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC) == 0 );
140  /* read ADC convertion result */
141  res += ADC_GetConversionValue(ADC1);
142  }
143 
144  /* de-initialize ADC */
145  ADC_TempSensorVrefintCmd(DISABLE);
146 
147  return (res>>2);
148 }
149 
150 uint16_t vref_read(void)
151 {
152  uint8_t i;
153  uint16_t res, T_StartupTimeDelay;
154 
155  adc_set_reg_seqlen(_ADC1, 1);
156  ADC_TempSensorVrefintCmd(ENABLE);
157 
158  /* Wait until ADC + Temp sensor start */
159  T_StartupTimeDelay = 1024;
160  while (T_StartupTimeDelay--);
161 
162  /* Enable TempSensor and Vrefint channels: channel16 and Channel17 */
163 // adc_channel_config(_adc1, ADC_Channel_17, 2, ADC_SampleTime_56Cycles);
165 
166  /* initialize result */
167  res = 0;
168  for(i=4; i>0; i--) {
169  /* start ADC convertion by software */
170  ADC_SoftwareStartConv(ADC1);
171 
172  /* wait until end-of-covertion */
173  while( ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC) == 0 );
174  /* read ADC convertion result */
175  res += ADC_GetConversionValue(ADC1);
176  }
177 
178  /* de-initialize ADC */
179  ADC_TempSensorVrefintCmd(DISABLE);
180 
181  return (res>>2);
182 }
183 #endif
const adc_dev const _adc3
Definition: adc.c:28
const adc_dev *const _ADC2
Definition: adc.c:26
uint16_t vref_read(void)
const adc_dev *const _ADC1
Definition: adc.c:20
ADC_TypeDef * adcx
Definition: adc.h:15
static void adc_channel_config(const adc_dev *dev, uint8_t channel, uint8_t rank, uint8_t sampleTime)
Definition: adc.h:104
#define ADC_Channel_16
Definition: adc.h:46
static void adc_enable(const adc_dev *dev)
Enable an adc peripheral.
Definition: adc.h:131
uint16_t adc_read(const adc_dev *dev, uint8_t channel)
static void adc_disable(const adc_dev *dev)
Disable an ADC peripheral.
Definition: adc.h:141
const adc_dev _adc2
Definition: adc.c:22
Analog-to-Digital Conversion (ADC) header.
const adc_dev _adc1
Definition: adc.c:16
static AP_HAL::OwnPtr< AP_HAL::Device > dev
Definition: ICM20789.cpp:16
uint16_t temp_read(void)
void adc_foreach(void(*fn)(const adc_dev *))
Call a function on all ADC devices.
Definition: adc.c:41
const adc_dev *const _ADC3
Definition: adc.c:32
void adc_init(const adc_dev *dev)
Initialize an ADC peripheral.
Definition: adc.c:56
#define ADC_Channel_17
Definition: adc.h:47
#define ADC_SampleTime_84Cycles
Definition: adc.h:58
Definition: adc.h:14
__IO uint16_t ADC_ConvertedValue
Definition: adc.c:34
#define ADC_SampleTime_56Cycles
Definition: adc.h:57
static void adc_set_reg_seqlen(const adc_dev *dev, uint8_t length)
Set the regular channel sequence length.
Definition: adc.h:86
__IO bool adc_data_ready
Definition: adc.c:35