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.

195 lines
5.8KB

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