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.

261 lines
7.4KB

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