10 роки тому
4 роки тому
4 роки тому
10 роки тому
4 роки тому
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429
  1. /* Audio Library for Teensy 3.X
  2. * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com
  3. *
  4. * Development of this audio library was funded by PJRC.COM, LLC by sales of
  5. * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop
  6. * open source software by purchasing Teensy or other PJRC products.
  7. *
  8. * Permission is hereby granted, free of charge, to any person obtaining a copy
  9. * of this software and associated documentation files (the "Software"), to deal
  10. * in the Software without restriction, including without limitation the rights
  11. * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  12. * copies of the Software, and to permit persons to whom the Software is
  13. * furnished to do so, subject to the following conditions:
  14. *
  15. * The above copyright notice, development funding notice, and this permission
  16. * notice shall be included in all copies or substantial portions of the Software.
  17. *
  18. * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  19. * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  20. * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  21. * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  22. * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  23. * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  24. * THE SOFTWARE.
  25. */
  26. #include <Arduino.h>
  27. #include "input_adc.h"
  28. #include "utility/dspinst.h"
  29. #if defined(KINETISK)
  30. #include "utility/pdb.h"
  31. #define COEF_HPF_DCBLOCK (1048300<<10) // DC Removal filter coefficient in S1.30
  32. DMAMEM __attribute__((aligned(32))) static uint16_t analog_rx_buffer[AUDIO_BLOCK_SAMPLES];
  33. audio_block_t * AudioInputAnalog::block_left = NULL;
  34. uint16_t AudioInputAnalog::block_offset = 0;
  35. int32_t AudioInputAnalog::hpf_y1 = 0;
  36. int32_t AudioInputAnalog::hpf_x1 = 0;
  37. bool AudioInputAnalog::update_responsibility = false;
  38. DMAChannel AudioInputAnalog::dma(false);
  39. void AudioInputAnalog::init(uint8_t pin)
  40. {
  41. int32_t tmp;
  42. // Configure the ADC and run at least one software-triggered
  43. // conversion. This completes the self calibration stuff and
  44. // leaves the ADC in a state that's mostly ready to use
  45. analogReadRes(16);
  46. analogReference(INTERNAL); // range 0 to 1.2 volts
  47. #if F_BUS == 96000000 || F_BUS == 48000000 || F_BUS == 24000000
  48. analogReadAveraging(8);
  49. #else
  50. analogReadAveraging(4);
  51. #endif
  52. // Note for review:
  53. // Probably not useful to spin cycles here stabilizing
  54. // since DC blocking is similar to te external analog filters
  55. tmp = (uint16_t) analogRead(pin);
  56. tmp = ( ((int32_t) tmp) << 14);
  57. hpf_x1 = tmp; // With constant DC level x1 would be x0
  58. hpf_y1 = 0; // Output will settle here when stable
  59. // set the programmable delay block to trigger the ADC at 44.1 kHz
  60. if (!(SIM_SCGC6 & SIM_SCGC6_PDB)
  61. || (PDB0_SC & PDB_CONFIG) != PDB_CONFIG
  62. || PDB0_MOD != PDB_PERIOD
  63. || PDB0_IDLY != 1
  64. || PDB0_CH0C1 != 0x0101) {
  65. SIM_SCGC6 |= SIM_SCGC6_PDB;
  66. PDB0_IDLY = 1;
  67. PDB0_MOD = PDB_PERIOD;
  68. PDB0_SC = PDB_CONFIG | PDB_SC_LDOK;
  69. PDB0_SC = PDB_CONFIG | PDB_SC_SWTRIG;
  70. PDB0_CH0C1 = 0x0101;
  71. }
  72. // enable the ADC for hardware trigger and DMA
  73. ADC0_SC2 |= ADC_SC2_ADTRG | ADC_SC2_DMAEN;
  74. // set up a DMA channel to store the ADC data
  75. dma.begin(true);
  76. dma.TCD->SADDR = &ADC0_RA;
  77. dma.TCD->SOFF = 0;
  78. dma.TCD->ATTR = DMA_TCD_ATTR_SSIZE(1) | DMA_TCD_ATTR_DSIZE(1);
  79. dma.TCD->NBYTES_MLNO = 2;
  80. dma.TCD->SLAST = 0;
  81. dma.TCD->DADDR = analog_rx_buffer;
  82. dma.TCD->DOFF = 2;
  83. dma.TCD->CITER_ELINKNO = sizeof(analog_rx_buffer) / 2;
  84. dma.TCD->DLASTSGA = -sizeof(analog_rx_buffer);
  85. dma.TCD->BITER_ELINKNO = sizeof(analog_rx_buffer) / 2;
  86. dma.TCD->CSR = DMA_TCD_CSR_INTHALF | DMA_TCD_CSR_INTMAJOR;
  87. dma.triggerAtHardwareEvent(DMAMUX_SOURCE_ADC0);
  88. update_responsibility = update_setup();
  89. dma.enable();
  90. dma.attachInterrupt(isr);
  91. }
  92. void AudioInputAnalog::isr(void)
  93. {
  94. uint32_t daddr, offset;
  95. const uint16_t *src, *end;
  96. uint16_t *dest_left;
  97. audio_block_t *left;
  98. daddr = (uint32_t)(dma.TCD->DADDR);
  99. dma.clearInterrupt();
  100. if (daddr < (uint32_t)analog_rx_buffer + sizeof(analog_rx_buffer) / 2) {
  101. // DMA is receiving to the first half of the buffer
  102. // need to remove data from the second half
  103. src = (uint16_t *)&analog_rx_buffer[AUDIO_BLOCK_SAMPLES/2];
  104. end = (uint16_t *)&analog_rx_buffer[AUDIO_BLOCK_SAMPLES];
  105. if (update_responsibility) AudioStream::update_all();
  106. } else {
  107. // DMA is receiving to the second half of the buffer
  108. // need to remove data from the first half
  109. src = (uint16_t *)&analog_rx_buffer[0];
  110. end = (uint16_t *)&analog_rx_buffer[AUDIO_BLOCK_SAMPLES/2];
  111. }
  112. left = block_left;
  113. if (left != NULL) {
  114. offset = block_offset;
  115. if (offset > AUDIO_BLOCK_SAMPLES/2) offset = AUDIO_BLOCK_SAMPLES/2;
  116. dest_left = (uint16_t *)&(left->data[offset]);
  117. block_offset = offset + AUDIO_BLOCK_SAMPLES/2;
  118. do {
  119. *dest_left++ = *src++;
  120. } while (src < end);
  121. }
  122. }
  123. void AudioInputAnalog::update(void)
  124. {
  125. audio_block_t *new_left=NULL, *out_left=NULL;
  126. uint32_t offset;
  127. int32_t tmp;
  128. int16_t s, *p, *end;
  129. //Serial.println("update");
  130. // allocate new block (ok if NULL)
  131. new_left = allocate();
  132. __disable_irq();
  133. offset = block_offset;
  134. if (offset < AUDIO_BLOCK_SAMPLES) {
  135. // the DMA didn't fill a block
  136. if (new_left != NULL) {
  137. // but we allocated a block
  138. if (block_left == NULL) {
  139. // the DMA doesn't have any blocks to fill, so
  140. // give it the one we just allocated
  141. block_left = new_left;
  142. block_offset = 0;
  143. __enable_irq();
  144. //Serial.println("fail1");
  145. } else {
  146. // the DMA already has blocks, doesn't need this
  147. __enable_irq();
  148. release(new_left);
  149. //Serial.print("fail2, offset=");
  150. //Serial.println(offset);
  151. }
  152. } else {
  153. // The DMA didn't fill a block, and we could not allocate
  154. // memory... the system is likely starving for memory!
  155. // Sadly, there's nothing we can do.
  156. __enable_irq();
  157. //Serial.println("fail3");
  158. }
  159. return;
  160. }
  161. // the DMA filled a block, so grab it and get the
  162. // new block to the DMA, as quickly as possible
  163. out_left = block_left;
  164. block_left = new_left;
  165. block_offset = 0;
  166. __enable_irq();
  167. //
  168. // DC Offset Removal Filter
  169. // 1-pole digital high-pass filter implementation
  170. // y = a*(x[n] - x[n-1] + y[n-1])
  171. // The coefficient "a" is as follows:
  172. // a = UNITY*e^(-2*pi*fc/fs)
  173. // fc = 2 @ fs = 44100
  174. //
  175. p = out_left->data;
  176. end = p + AUDIO_BLOCK_SAMPLES;
  177. do {
  178. tmp = (uint16_t)(*p);
  179. tmp = ( ((int32_t) tmp) << 14);
  180. int32_t acc = hpf_y1 - hpf_x1;
  181. acc += tmp;
  182. hpf_y1 = FRACMUL_SHL(acc, COEF_HPF_DCBLOCK, 1);
  183. hpf_x1 = tmp;
  184. s = signed_saturate_rshift(hpf_y1, 16, 14);
  185. *p++ = s;
  186. } while (p < end);
  187. // then transmit the AC data
  188. transmit(out_left);
  189. release(out_left);
  190. }
  191. #endif
  192. #if defined(__IMXRT1062__)
  193. #include <Arduino.h>
  194. #include "input_adc.h"
  195. extern "C" void xbar_connect(unsigned int input, unsigned int output);
  196. #define FILTERLEN 15
  197. DMAChannel AudioInputAnalog::dma(false);
  198. // TODO: how much extra space is needed to avoid wrap-around timing? 200 seems a safe guess
  199. static __attribute__((aligned(32))) uint16_t adc_buffer[AUDIO_BLOCK_SAMPLES*4+200];
  200. static int16_t capture_buffer[AUDIO_BLOCK_SAMPLES*4+FILTERLEN];
  201. // TODO: these big buffers should be in DMAMEM, rather than consuming precious DTCM
  202. PROGMEM static const uint8_t adc2_pin_to_channel[] = {
  203. 7, // 0/A0 AD_B1_02
  204. 8, // 1/A1 AD_B1_03
  205. 12, // 2/A2 AD_B1_07
  206. 11, // 3/A3 AD_B1_06
  207. 6, // 4/A4 AD_B1_01
  208. 5, // 5/A5 AD_B1_00
  209. 15, // 6/A6 AD_B1_10
  210. 0, // 7/A7 AD_B1_11
  211. 13, // 8/A8 AD_B1_08
  212. 14, // 9/A9 AD_B1_09
  213. 255, // 10/A10 AD_B0_12 - only on ADC1, 1 - can't use for audio
  214. 255, // 11/A11 AD_B0_13 - only on ADC1, 2 - can't use for audio
  215. 3, // 12/A12 AD_B1_14
  216. 4, // 13/A13 AD_B1_15
  217. 7, // 14/A0 AD_B1_02
  218. 8, // 15/A1 AD_B1_03
  219. 12, // 16/A2 AD_B1_07
  220. 11, // 17/A3 AD_B1_06
  221. 6, // 18/A4 AD_B1_01
  222. 5, // 19/A5 AD_B1_00
  223. 15, // 20/A6 AD_B1_10
  224. 0, // 21/A7 AD_B1_11
  225. 13, // 22/A8 AD_B1_08
  226. 14, // 23/A9 AD_B1_09
  227. 255, // 24/A10 AD_B0_12 - only on ADC1, 1 - can't use for audio
  228. 255, // 25/A11 AD_B0_13 - only on ADC1, 2 - can't use for audio
  229. 3, // 26/A12 AD_B1_14 - only on ADC2, do not use analogRead()
  230. 4, // 27/A13 AD_B1_15 - only on ADC2, do not use analogRead()
  231. #ifdef ARDUINO_TEENSY41
  232. 255, // 28
  233. 255, // 29
  234. 255, // 30
  235. 255, // 31
  236. 255, // 32
  237. 255, // 33
  238. 255, // 34
  239. 255, // 35
  240. 255, // 36
  241. 255, // 37
  242. 1, // 38/A14 AD_B1_12 - only on ADC2, do not use analogRead()
  243. 2, // 39/A15 AD_B1_13 - only on ADC2, do not use analogRead()
  244. 9, // 40/A16 AD_B1_04
  245. 10, // 41/A17 AD_B1_05
  246. #endif
  247. };
  248. static const int16_t filter[FILTERLEN] = {
  249. 1449,
  250. 3676,
  251. 6137,
  252. 9966,
  253. 13387,
  254. 16896,
  255. 18951,
  256. 19957,
  257. 18951,
  258. 16896,
  259. 13387,
  260. 9966,
  261. 6137,
  262. 3676,
  263. 1449
  264. };
  265. void AudioInputAnalog::init(uint8_t pin)
  266. {
  267. if (pin >= sizeof(adc2_pin_to_channel)) return;
  268. const uint8_t adc_channel = adc2_pin_to_channel[pin];
  269. if (adc_channel == 255) return;
  270. // configure a timer to trigger ADC
  271. // TODO: sample rate should be slightly lower than 4X AUDIO_SAMPLE_RATE_EXACT
  272. // linear interpolation is supposed to resample it to exactly 4X
  273. // the sample rate, so we avoid artifacts boundaries between captures
  274. const int comp1 = ((float)F_BUS_ACTUAL) / (AUDIO_SAMPLE_RATE_EXACT * 4.0f) / 2.0f + 0.5f;
  275. TMR4_ENBL &= ~(1<<3);
  276. TMR4_SCTRL3 = TMR_SCTRL_OEN | TMR_SCTRL_FORCE;
  277. TMR4_CSCTRL3 = TMR_CSCTRL_CL1(1) | TMR_CSCTRL_TCF1EN;
  278. TMR4_CNTR3 = 0;
  279. TMR4_LOAD3 = 0;
  280. TMR4_COMP13 = comp1;
  281. TMR4_CMPLD13 = comp1;
  282. TMR4_CTRL3 = TMR_CTRL_CM(1) | TMR_CTRL_PCS(8) | TMR_CTRL_LENGTH | TMR_CTRL_OUTMODE(3);
  283. TMR4_DMA3 = TMR_DMA_CMPLD1DE;
  284. TMR4_CNTR3 = 0;
  285. TMR4_ENBL |= (1<<3);
  286. // connect the timer output the ADC_ETC input
  287. const int trigger = 4; // 0-3 for ADC1, 4-7 for ADC2
  288. CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON);
  289. xbar_connect(XBARA1_IN_QTIMER4_TIMER3, XBARA1_OUT_ADC_ETC_TRIG00 + trigger);
  290. // turn on ADC_ETC and configure to receive trigger
  291. if (ADC_ETC_CTRL & (ADC_ETC_CTRL_SOFTRST | ADC_ETC_CTRL_TSC_BYPASS)) {
  292. ADC_ETC_CTRL = 0; // clears SOFTRST only
  293. ADC_ETC_CTRL = 0; // clears TSC_BYPASS
  294. }
  295. ADC_ETC_CTRL |= ADC_ETC_CTRL_TRIG_ENABLE(1 << trigger) | ADC_ETC_CTRL_DMA_MODE_SEL;
  296. ADC_ETC_DMA_CTRL |= ADC_ETC_DMA_CTRL_TRIQ_ENABLE(trigger);
  297. // configure ADC_ETC trigger4 to make one ADC2 measurement on pin A2
  298. const int len = 1;
  299. IMXRT_ADC_ETC.TRIG[trigger].CTRL = ADC_ETC_TRIG_CTRL_TRIG_CHAIN(len - 1) |
  300. ADC_ETC_TRIG_CTRL_TRIG_PRIORITY(7);
  301. IMXRT_ADC_ETC.TRIG[trigger].CHAIN_1_0 = ADC_ETC_TRIG_CHAIN_HWTS0(1) |
  302. ADC_ETC_TRIG_CHAIN_CSEL0(adc2_pin_to_channel[pin]) | ADC_ETC_TRIG_CHAIN_B2B0;
  303. // set up ADC2 for 12 bit mode, hardware trigger
  304. Serial.printf("ADC2_CFG = %08X\n", ADC2_CFG);
  305. ADC2_CFG |= ADC_CFG_ADTRG;
  306. ADC2_CFG = ADC_CFG_MODE(2) | ADC_CFG_ADSTS(3) | ADC_CFG_ADLSMP | ADC_CFG_ADTRG |
  307. ADC_CFG_ADICLK(1) | ADC_CFG_ADIV(0) /*| ADC_CFG_ADHSC*/;
  308. ADC2_GC &= ~ADC_GC_AVGE; // single sample, no averaging
  309. ADC2_HC0 = ADC_HC_ADCH(16); // 16 = controlled by ADC_ETC
  310. // use a DMA channel to capture ADC_ETC output
  311. dma.begin();
  312. dma.TCD->SADDR = &(IMXRT_ADC_ETC.TRIG[4].RESULT_1_0);
  313. dma.TCD->SOFF = 0;
  314. dma.TCD->ATTR = DMA_TCD_ATTR_SSIZE(1) | DMA_TCD_ATTR_DSIZE(1);
  315. dma.TCD->NBYTES_MLNO = 2;
  316. dma.TCD->SLAST = 0;
  317. dma.TCD->DADDR = adc_buffer;
  318. dma.TCD->DOFF = 2;
  319. dma.TCD->CITER_ELINKNO = sizeof(adc_buffer) / 2;
  320. dma.TCD->DLASTSGA = -sizeof(adc_buffer);
  321. dma.TCD->BITER_ELINKNO = sizeof(adc_buffer) / 2;
  322. dma.TCD->CSR = 0;
  323. dma.triggerAtHardwareEvent(DMAMUX_SOURCE_ADC_ETC);
  324. dma.enable();
  325. // TODO: configure I2S1 to interrupt every 128 audio samples
  326. }
  327. static int16_t fir(const int16_t *data, const int16_t *impulse, int len)
  328. {
  329. int64_t sum=0;
  330. while (len > 0) {
  331. sum += *data++ * *impulse++; // TODO: optimize with DSP inst and filter symmetry
  332. len --;
  333. }
  334. sum = sum >> 15; // TODO: adjust filter coefficients for proper gain, 12 to 16 bits
  335. if (sum > 32767) return 32767;
  336. if (sum < -32768) return -32768;
  337. return sum;
  338. }
  339. void AudioInputAnalog::update(void)
  340. {
  341. audio_block_t *output=NULL;
  342. output = allocate();
  343. if (output == NULL) return;
  344. uint16_t *p = (uint16_t *)dma.TCD->DADDR;
  345. //int offset = p - adc_buffer;
  346. //if (--offset < 0) offset = sizeof(adc_buffer) / 2 - 1;
  347. //Serial.printf("offset = %4d, val = %4d\n", offset + 1, adc_buffer[offset]);
  348. // copy adc buffer to capture buffer
  349. // FIXME: this should be done from the I2S interrupt, for precise capture timing
  350. const unsigned int capture_len = sizeof(capture_buffer) / 2;
  351. for (unsigned int i=0; i < capture_len; i++) {
  352. // TODO: linear interpolate to exactly 4X sample rate
  353. if (--p < adc_buffer) p = adc_buffer + (sizeof(adc_buffer) / 2 - 1);
  354. // remove DC offset
  355. // TODO: very slow low pass filter for DC offset
  356. int dc_offset = 550; // FIXME: quick kludge for testing!!
  357. int n = (int)*p - dc_offset;
  358. if (n > 4095) n = 4095;
  359. if (n < -4095) n = -4095;
  360. capture_buffer[i] = n;
  361. }
  362. //printbuf(capture_buffer, 8);
  363. // low pass filter and subsample (this part belongs here)
  364. int16_t *dest = output->data + AUDIO_BLOCK_SAMPLES - 1;
  365. for (int i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
  366. #if 1
  367. // proper low-pass filter sounds pretty good
  368. *dest-- = fir(capture_buffer + i * 4, filter, sizeof(filter)/2);
  369. #else
  370. // just averge 4 samples together, lower quality but much faster
  371. *dest-- = capture_buffer[i * 4] + capture_buffer[i * 4 + 1]
  372. + capture_buffer[i * 4 + 2] + capture_buffer[i * 4 + 3];
  373. #endif
  374. }
  375. transmit(output);
  376. release(output);
  377. }
  378. #endif