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.

258 lines
7.6KB

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