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.

239 lines
7.2KB

  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. }