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.

159 lines
4.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 "AT91SAM7S256.h"
  20. #include "at91_sysc.h"
  21. #include "at91_dbgu.h"
  22. #include "at91_pitc.h"
  23. #include "at91_adc.h"
  24. #include "at91_tests.h"
  25. #include "at91_udp.h"
  26. #include "at91_pio.h"
  27. #include "at91_twi.h"
  28. #include "at91_tc1.h"
  29. #include "memalloc.h"
  30. #include "board.h"
  31. #include <stdio.h>
  32. #include <string.h>
  33. static uint32_t global_state;
  34. #define MOTORS_RUNNING 0x0001
  35. #define STICK_CALIBRATION 0x0002
  36. static uint32_t stick_timer_cb(struct pitc_timer *timer)
  37. {
  38. static uint32_t i;
  39. *AT91C_PIOA_SODR = i;
  40. i = i ^ LED_GREEN;
  41. *AT91C_PIOA_CODR = i;
  42. struct rc_values rc;
  43. uint32_t count = rcontrol_getswitches(&rc);
  44. static uint32_t stick_cal_count;
  45. if (global_state & MOTORS_RUNNING) {
  46. if ((rc.chan[2] < 0 && rc.chan[3] > 0) || count < 4)
  47. global_state &= ~MOTORS_RUNNING;
  48. } else if (global_state & STICK_CALIBRATION) {
  49. stick_cal_count--;
  50. if (stick_cal_count == 0) {
  51. rcontrol_calibrate(RC_CAL_END);
  52. rcontrol_calibrate(RC_CAL_SAVE);
  53. rcontrol_print_cal();
  54. global_state &= ~STICK_CALIBRATION;
  55. }
  56. } else {
  57. if (rc.chan[2] < 0 && rc.chan[3] < 0)
  58. global_state |= MOTORS_RUNNING;
  59. if (rc.chan[2] > 0 && rc.chan[3] > 0)
  60. adc_calibrate(ADC_CAL_GYRO);
  61. if (rc.chan[2] > 0 && rc.chan[3] < 0) {
  62. adc_calibrate(ADC_CAL_ACC);
  63. stick_cal_count = 3000;
  64. global_state |= STICK_CALIBRATION;
  65. rcontrol_calibrate(RC_CAL_START);
  66. }
  67. }
  68. rcontrol_getvalues(&rc);
  69. static uint8_t pwm[4];
  70. if (global_state & MOTORS_RUNNING) {
  71. int32_t pitch = rc.chan[2] * 256;
  72. int32_t gier = rc.chan[3] * 64;
  73. int32_t nick = rc.chan[0] * 64;
  74. int32_t roll = rc.chan[1] * 64;
  75. int32_t data[4];
  76. data[0] = (pitch - nick + gier) / 1024;
  77. data[1] = (pitch + nick + gier) / 1024;
  78. data[2] = (pitch + roll - gier) / 1024;
  79. data[3] = (pitch - roll - gier) / 1024;
  80. pwm[0] = (data[0] > 0x0f) ? data[0] : 0x0f;
  81. pwm[1] = (data[1] > 0x0f) ? data[1] : 0x0f;
  82. pwm[2] = (data[2] > 0x0f) ? data[2] : 0x0f;
  83. pwm[3] = (data[3] > 0x0f) ? data[3] : 0x0f;
  84. } else {
  85. pwm[0] = 0x00;
  86. pwm[1] = 0x00;
  87. pwm[2] = 0x00;
  88. pwm[3] = 0x00;
  89. }
  90. // printf("%3d %3d %3d %3d\n\r", pwm[0], pwm[1], pwm[2], pwm[3]);
  91. twi_setpwm(pwm);
  92. // adc_trigger();
  93. return PITC_RESTART_TIMER;
  94. }
  95. static struct pitc_timer stick_timer = {
  96. .interval = 10,
  97. .func = &stick_timer_cb,
  98. };
  99. int main(void)
  100. {
  101. /* LED outputs */
  102. *AT91C_PIOA_PER = LED_GREEN | LED_ORANGE;
  103. *AT91C_PIOA_OER = LED_GREEN | LED_ORANGE;
  104. /* needed for dbgu */
  105. at91_sysc_init();
  106. at91_dbgu_init();
  107. at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r");
  108. /* triggers pinchange-isrs */
  109. at91_pio_init();
  110. /* timer */
  111. at91_pitc_init();
  112. at91_rttc_test_init();
  113. /* twi */
  114. at91_twi_init();
  115. at91_twi_test();
  116. /* remote control, needs twi */
  117. at91_tc1_init();
  118. /* usb */
  119. at91_udp_init();
  120. /* adc, need timer, twi */
  121. at91_adc_init();
  122. pitc_schedule_timer(&stick_timer);
  123. printf("static alloc: %5ld bytes\n\r", static_alloc_used());
  124. while (1);
  125. }