ARM7 based quadrocopter
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

at91_adc.c 7.2KB

12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
12 years ago
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238
  1. /***************************************************************************
  2. * sam7fc - ADC routines / calibration *
  3. * *
  4. * Copyright (C) 01/2008 by Olaf Rempel *
  5. * razzor@kopf-tisch.de *
  6. * *
  7. * This program is free software; you can redistribute it and/or modify *
  8. * it under the terms of the GNU General Public License as published by *
  9. * the Free Software Foundation; version 2 of the License *
  10. * *
  11. * This program is distributed in the hope that it will be useful, *
  12. * but WITHOUT ANY WARRANTY; without even the implied warranty of *
  13. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
  14. * GNU General Public License for more details. *
  15. * *
  16. * You should have received a copy of the GNU General Public License *
  17. * along with this program; if not, write to the *
  18. * Free Software Foundation, Inc., *
  19. * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
  20. ***************************************************************************/
  21. #include <stdint.h>
  22. #include <stdio.h>
  23. #include "AT91SAM7S256.h"
  24. #include "board.h"
  25. #include "at91_adc.h"
  26. #include "at91_pitc.h"
  27. #include "at91_twi.h"
  28. #include "telemetrie.h"
  29. static uint32_t adc_status;
  30. static uint16_t adc_tmp[7];
  31. static uint16_t adc_offset[6];
  32. TDC_PTR(adc_offset0, &adc_offset[ADC_GYRO_NICK], "XADC_GYRO_NICK (offset)", int16_t, TDC_UNSIGNED);
  33. TDC_PTR(adc_offset1, &adc_offset[ADC_GYRO_ROLL], "XADC_GYRO_ROLL (offset)", int16_t, TDC_UNSIGNED);
  34. TDC_PTR(adc_offset2, &adc_offset[ADC_GYRO_GIER], "XADC_GYRO_GIER (offset)", int16_t, TDC_UNSIGNED);
  35. TDC_PTR(adc_offset3, &adc_offset[ADC_ACC_NICK], "XADC_ACC_NICK (offset)", int16_t, TDC_UNSIGNED);
  36. TDC_PTR(adc_offset4, &adc_offset[ADC_ACC_ROLL], "XADC_ACC_ROLL (offset)", int16_t, TDC_UNSIGNED);
  37. TDC_PTR(adc_offset5, &adc_offset[ADC_ACC_GIER], "XADC_ACC_GIER (offset)", int16_t, TDC_UNSIGNED);
  38. #define ADC_CAL_COUNT_MAX 1024
  39. static uint32_t adc_cal_count;
  40. static uint32_t adc_cal_data[3];
  41. /* check eeprom parameter size (3x uint16_t) */
  42. #if ((3 * 2) != EE_ACC_CAL_DATA_SIZE)
  43. #error "invalid EE_ACC_CAL_DATA_SIZE"
  44. #endif
  45. static void at91_adc_isr(void)
  46. {
  47. AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
  48. pdc->PDC_RPR = (uint32_t) &adc_tmp;
  49. pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
  50. pdc->PDC_PTCR = AT91C_PDC_RXTEN;
  51. /* clear interrupts */
  52. uint32_t dummy = *AT91C_ADC_SR;
  53. dummy = dummy;
  54. if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC)))
  55. return;
  56. if (adc_status & ADC_CAL_GYRO) {
  57. adc_cal_data[0] += adc_tmp[ADC_GYRO_NICK];
  58. adc_cal_data[1] += adc_tmp[ADC_GYRO_ROLL];
  59. adc_cal_data[2] += adc_tmp[ADC_GYRO_GIER];
  60. } else {
  61. adc_cal_data[0] += adc_tmp[ADC_ACC_NICK];
  62. adc_cal_data[1] += adc_tmp[ADC_ACC_ROLL];
  63. adc_cal_data[2] += adc_tmp[ADC_ACC_GIER];
  64. }
  65. adc_cal_count++;
  66. }
  67. static uint32_t adc_calibrate_cb(struct pitc_timer *timer)
  68. {
  69. if (adc_cal_count < ADC_CAL_COUNT_MAX) {
  70. /* trigger next cycle */
  71. *AT91C_ADC_CR = AT91C_ADC_START;
  72. return PITC_RESTART_TIMER;
  73. } else {
  74. if (adc_status & ADC_CAL_GYRO) {
  75. adc_offset[ADC_GYRO_NICK] = adc_cal_data[0] / ADC_CAL_COUNT_MAX;
  76. adc_offset[ADC_GYRO_ROLL] = adc_cal_data[1] / ADC_CAL_COUNT_MAX;
  77. adc_offset[ADC_GYRO_GIER] = adc_cal_data[2] / ADC_CAL_COUNT_MAX;
  78. adc_calibrate(ADC_CAL_GYRO_COMPLETE);
  79. } else {
  80. adc_offset[ADC_ACC_NICK] = adc_cal_data[0] / ADC_CAL_COUNT_MAX;
  81. adc_offset[ADC_ACC_ROLL] = adc_cal_data[1] / ADC_CAL_COUNT_MAX;
  82. adc_offset[ADC_ACC_GIER] = adc_cal_data[2] / ADC_CAL_COUNT_MAX;
  83. adc_calibrate(ADC_CAL_ACC_COMPLETE);
  84. }
  85. return PITC_REMOVE_TIMER;
  86. }
  87. }
  88. static struct pitc_timer adc_cal_timer = {
  89. .interval = 1,
  90. .func = &adc_calibrate_cb,
  91. };
  92. void adc_trigger(void)
  93. {
  94. // TODO: err in retvalue?
  95. if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC)))
  96. *AT91C_ADC_CR = AT91C_ADC_START;
  97. }
  98. void adc_get_results(int16_t *adc_result)
  99. {
  100. // TODO: err in retvalue?
  101. if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) {
  102. uint32_t i;
  103. for (i = ADC_GYRO_NICK; i <= ADC_GYRO_GIER; i++)
  104. adc_result[i] = (int16_t)(adc_offset[i]) - (int16_t)(adc_tmp[i]);
  105. for (i = ADC_ACC_NICK; i <= ADC_ACC_GIER; i++)
  106. adc_result[i] = (int16_t)(adc_tmp[i]) - (int16_t)(adc_offset[i]);
  107. /* (adc / 1024) * 3.3V * (11k / 1k) * 100 */
  108. adc_result[ADC_VOLTAGE] = ((uint32_t)adc_tmp[ADC_VOLTAGE] * 3630) / 1024;
  109. }
  110. }
  111. void adc_calibrate(uint32_t mode)
  112. {
  113. /* disable interrupt */
  114. *AT91C_ADC_IDR = AT91C_ADC_ENDRX;
  115. switch (mode) {
  116. case ADC_CAL_GYRO_COMPLETE:
  117. adc_status &= ~ADC_CAL_GYRO;
  118. break;
  119. case ADC_CAL_ACC_COMPLETE:
  120. twi_write_eeprom(EE_ACC_CAL_DATA,
  121. (uint8_t *)&(adc_offset[ADC_ACC_NICK]),
  122. EE_ACC_CAL_DATA_SIZE);
  123. adc_status &= ~ADC_CAL_ACC;
  124. break;
  125. case ADC_CAL_ACC_LOAD:
  126. twi_read_eeprom(EE_ACC_CAL_DATA,
  127. (uint8_t *)&(adc_offset[ADC_ACC_NICK]),
  128. EE_ACC_CAL_DATA_SIZE);
  129. break;
  130. case ADC_CAL_GYRO:
  131. case ADC_CAL_ACC:
  132. /* calibration in progress.. */
  133. if (adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))
  134. break;
  135. adc_status |= mode;
  136. adc_cal_count = 0;
  137. adc_cal_data[0] = 0;
  138. adc_cal_data[1] = 0;
  139. adc_cal_data[2] = 0;
  140. pitc_schedule_timer(&adc_cal_timer);
  141. /* trigger next cycle */
  142. *AT91C_ADC_CR = AT91C_ADC_START;
  143. break;
  144. }
  145. if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) {
  146. printf("ADC offsets: %d/%d/%d %d/%d/%d\n\r",
  147. adc_offset[ADC_GYRO_NICK],
  148. adc_offset[ADC_GYRO_ROLL],
  149. adc_offset[ADC_GYRO_GIER],
  150. adc_offset[ADC_ACC_NICK],
  151. adc_offset[ADC_ACC_ROLL],
  152. adc_offset[ADC_ACC_GIER]);
  153. }
  154. /* enable interrupt */
  155. *AT91C_ADC_IER = AT91C_ADC_ENDRX;
  156. }
  157. void adc_drift_adjust(int16_t nick, int16_t roll, int16_t yaw)
  158. {
  159. adc_offset[ADC_GYRO_NICK] += nick;
  160. adc_offset[ADC_GYRO_ROLL] += roll;
  161. adc_offset[ADC_GYRO_GIER] += yaw;
  162. }
  163. void at91_adc_init(void)
  164. {
  165. /* enable ADC clock */
  166. *AT91C_PMC_PCER = (1 << AT91C_ID_ADC);
  167. /* ADC Software reset */
  168. AT91S_ADC *adc = AT91C_BASE_ADC;
  169. adc->ADC_CR = AT91C_ADC_SWRST;
  170. /*
  171. * ADC config: 10bit, no sleep
  172. * 4.8MHz (48MHz / ((4 +1) * 2) = 4.8MHz)
  173. * 96 cycles Startup ((11 +1) * 8 / 4.8MHz = 20us)
  174. * 3 cycles SH ((2 +1) / 4.8MHz = 625ns)
  175. * Conversion time per channel @5MHz ~2us
  176. */
  177. adc->ADC_MR = AT91C_ADC_TRGEN_DIS |
  178. AT91C_ADC_LOWRES_10_BIT |
  179. AT91C_ADC_SLEEP_NORMAL_MODE |
  180. (AT91C_ADC_PRESCAL & (4 << 8)) |
  181. (AT91C_ADC_STARTUP & (11 << 16)) |
  182. (AT91C_ADC_SHTIM & (2 << 24));
  183. /* setup PDC */
  184. AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
  185. pdc->PDC_RPR = (uint32_t) &adc_tmp;
  186. pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
  187. pdc->PDC_PTCR = AT91C_PDC_RXTEN;
  188. /* enable 7 channels (0-1-2-4-5-6-7), PDC Interrupt */
  189. adc->ADC_CHER = 0xF7;
  190. adc->ADC_IER = AT91C_ADC_ENDRX;
  191. /* low priority, level triggered, own vector */
  192. AT91S_AIC *aic = AT91C_BASE_AIC;
  193. aic->AIC_SMR[AT91C_ID_ADC] = IRQPRIO_ADC | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
  194. aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr;
  195. aic->AIC_IECR = (1<<AT91C_ID_ADC);
  196. adc_calibrate(ADC_CAL_ACC_LOAD);
  197. adc_calibrate(ADC_CAL_GYRO);
  198. }