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_tc1.c 7.6KB

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
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
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257
  1. /***************************************************************************
  2. * sam7fc - RC-PPM Signal decoder *
  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 <stdlib.h> /* abs() */
  24. #include "AT91SAM7S256.h"
  25. #include "board.h"
  26. #include "at91_tc1.h"
  27. #include "at91_twi.h"
  28. /* Hard limits for ISR */
  29. #define PULSE_MIN 0x0500
  30. #define PULSE_MAX 0x0D00
  31. #define PULSE_TIMEOUT 0x0F00
  32. #define PULSE_CENTER 0x08C0
  33. #define PULSE_SWITCH 0x01F0
  34. /* moving average filters */
  35. #define PULSE_FILTER_FAST (1<<2)
  36. #define PULSE_FILTER_SLOW (1<<4)
  37. #define PULSE_FILTER_DIFF 16 /* point to switch filters */
  38. #define PULSE_MID_DIFF 50 /* minimum diff to center */
  39. #define VALUE_RANGE 256
  40. #define ROUND_DIV256(x) ((x >> 8) + ((x & 0x80) ? 1 : 0))
  41. struct channel_data {
  42. uint16_t width;
  43. uint16_t width_slow;
  44. uint16_t filter; /* 0 - fast filter, 1 - slow filter */
  45. uint16_t min; /* minimum value during calibration */
  46. uint16_t mid; /* center value */
  47. uint16_t max; /* maximum value */
  48. };
  49. /* check eeprom parameter size (uint16_t min/mid/max) */
  50. #if ((MAX_CHANNELS * 3 * 2) != EE_RC_CAL_DATA_SIZE)
  51. #error "invalid EE_RC_CAL_DATA_SIZE"
  52. #endif
  53. static struct channel_data ch_data[MAX_CHANNELS];
  54. static uint32_t count, valid, cal_in_progress;
  55. static void ppm_isr(void)
  56. {
  57. static uint32_t i;
  58. /* RC Compare -> no TIOA1 edge for 2.5ms */
  59. uint32_t status = *AT91C_TC1_SR;
  60. if (status & AT91C_TC_CPCS) {
  61. /* average channel count */
  62. count = ((count * 7) + (i << 8)) / 8;
  63. /* at least 4 channels and a stable channel count */
  64. if ((ROUND_DIV256(count) == i) && (i >= 4)) {
  65. if (valid < 10)
  66. valid++;
  67. } else if (valid > 0) {
  68. valid--;
  69. }
  70. /* reset index */
  71. i = 0;
  72. }
  73. /* edge on TIOA1 */
  74. if (status & AT91C_TC_LDRAS) {
  75. /* get impulse width */
  76. uint16_t width = *AT91C_TC1_RA;
  77. /* valid range: 1 - 2ms */
  78. if (width > PULSE_MIN && width < PULSE_MAX) {
  79. if (i < ARRAY_SIZE(ch_data)) {
  80. /* calc both filters */
  81. ch_data[i].width = ((ch_data[i].width * (PULSE_FILTER_FAST -1)) + width) / PULSE_FILTER_FAST;
  82. ch_data[i].width_slow = ((ch_data[i].width_slow * (PULSE_FILTER_SLOW -1)) + width) / PULSE_FILTER_SLOW;
  83. if (cal_in_progress) {
  84. /* use slow filter values, calc center */
  85. ch_data[i].min = MIN(ch_data[i].width_slow, ch_data[i].min);
  86. ch_data[i].max = MAX(ch_data[i].width_slow, ch_data[i].max);
  87. ch_data[i].mid = (ch_data[i].min + ch_data[i].max) / 2;
  88. }
  89. }
  90. i++;
  91. }
  92. }
  93. }
  94. uint32_t rcontrol_getvalues(struct rc_values *rc)
  95. {
  96. if (valid < 5)
  97. return 0;
  98. uint32_t i;
  99. uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
  100. for (i = 0; i < cnt; i++) {
  101. /* switch between fast and slow filter */
  102. uint16_t filter = (abs(ch_data[i].width - ch_data[i].width_slow) < PULSE_FILTER_DIFF);
  103. /*
  104. * transition fast -> slow filter
  105. * slow filter is lagging behind, so give it a boost
  106. */
  107. if (filter && !ch_data[i].filter && !cal_in_progress)
  108. ch_data[i].width_slow = ch_data[i].width;
  109. ch_data[i].filter = filter;
  110. uint16_t width = (filter) ? ch_data[i].width_slow : ch_data[i].width;
  111. /* expand the value to +/- VALUE_RANGE */
  112. int32_t tmp = (uint32_t)(width - ch_data[i].mid) * VALUE_RANGE;
  113. tmp = tmp / ((tmp > 0) ? (ch_data[i].max - ch_data[i].mid) : (ch_data[i].mid - ch_data[i].min));
  114. // TODO: stick mapping
  115. /* keep result in range */
  116. rc->chan[i] = LIMIT(tmp, -VALUE_RANGE, +VALUE_RANGE);
  117. }
  118. return cnt;
  119. }
  120. uint32_t rcontrol_getswitches(struct rc_values *rc)
  121. {
  122. if (valid < 5)
  123. return 0;
  124. uint32_t i;
  125. uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
  126. for (i = 0; i < cnt; i++) {
  127. if (ch_data[i].width > (PULSE_CENTER + PULSE_SWITCH))
  128. rc->chan[i] = VALUE_RANGE;
  129. else if (ch_data[i].width < (PULSE_CENTER - PULSE_SWITCH))
  130. rc->chan[i] = -VALUE_RANGE;
  131. else
  132. rc->chan[i] = 0;
  133. }
  134. return cnt;
  135. }
  136. void rcontrol_calibrate(uint32_t mode)
  137. {
  138. uint32_t i;
  139. uint8_t buf[EE_RC_CAL_DATA_SIZE];
  140. uint16_t *ptr = (uint16_t *)buf;
  141. switch (mode) {
  142. case RC_CAL_START:
  143. cal_in_progress = 1;
  144. for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
  145. /* use hard limits as hint */
  146. ch_data[i].max = PULSE_MIN;
  147. ch_data[i].mid = (PULSE_MIN + PULSE_MAX) / 2;
  148. ch_data[i].min = PULSE_MAX;
  149. }
  150. break;
  151. case RC_CAL_END:
  152. cal_in_progress = 0;
  153. for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
  154. /* treat current position as center */
  155. ch_data[i].mid = ch_data[i].width_slow;
  156. /* if center is near minimum, clamp output to 0..+RANGE */
  157. if (ch_data[i].mid - ch_data[i].min < PULSE_MID_DIFF)
  158. ch_data[i].mid = ch_data[i].min;
  159. /* if center is near maximum, clamp output to -RANGE..0 */
  160. if (ch_data[i].max - ch_data[i].mid < PULSE_MID_DIFF)
  161. ch_data[i].mid = ch_data[i].max;
  162. }
  163. break;
  164. case RC_CAL_LOAD:
  165. twi_read_eeprom(EE_RC_CAL_DATA, buf, EE_RC_CAL_DATA_SIZE);
  166. for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
  167. ch_data[i].min = *ptr++;
  168. ch_data[i].mid = *ptr++;
  169. ch_data[i].max = *ptr++;
  170. }
  171. break;
  172. case RC_CAL_SAVE:
  173. for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
  174. *ptr++ = ch_data[i].min;
  175. *ptr++ = ch_data[i].mid;
  176. *ptr++ = ch_data[i].max;
  177. }
  178. twi_write_eeprom(EE_RC_CAL_DATA, buf, EE_RC_CAL_DATA_SIZE);
  179. break;
  180. }
  181. }
  182. void rcontrol_print_cal(void)
  183. {
  184. uint32_t i;
  185. printf("stick-calibration:\n\r");
  186. for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
  187. printf(" %ld: %d(%+d) - %d(0) - %d(%+d)\n\r", i,
  188. ch_data[i].min, ch_data[i].min - ch_data[i].mid,
  189. ch_data[i].mid,
  190. ch_data[i].max, ch_data[i].max - ch_data[i].mid
  191. );
  192. }
  193. }
  194. void at91_tc1_init(void)
  195. {
  196. /* enable TC1 clock */
  197. *AT91C_PMC_PCER = (1 << AT91C_ID_TC1);
  198. /* MCK /32, trigger & capture on falling TIOA1 edge */
  199. *AT91C_TC1_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | AT91C_TC_LDRA_FALLING |
  200. AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG;
  201. /* enable RA load and RC compare interrupt */
  202. *AT91C_TC1_IER = AT91C_TC_LDRAS | AT91C_TC_CPCS;
  203. /* RC Compare Interrupt if no rising Edge on TIOA1 for 2.56ms */
  204. *AT91C_TC1_RC = PULSE_TIMEOUT;
  205. /* enable & trigger the clock */
  206. *AT91C_TC1_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
  207. /* level triggered, own vector */
  208. AT91S_AIC *aic = AT91C_BASE_AIC;
  209. aic->AIC_SMR[AT91C_ID_TC1] = IRQPRIO_TC1 | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
  210. aic->AIC_SVR[AT91C_ID_TC1] = (uint32_t)ppm_isr;
  211. aic->AIC_IECR = (1 << AT91C_ID_TC1);
  212. rcontrol_calibrate(RC_CAL_LOAD);
  213. rcontrol_print_cal();
  214. }