Subversion Repositories FlightCtrl

Rev

Rev 1910 | Rev 2102 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1910 Rev 2099
Line 6... Line 6...
6 
#include "timer0.h"
 6 
#include "timer0.h"
7 
#include "timer2.h"
 7 
#include "timer2.h"
8 
#include "uart0.h"
 8 
#include "uart0.h"
9 
#include "output.h"
 9 
#include "output.h"
10 
#include "attitude.h"
 10 
#include "attitude.h"
- 
 
 11 
#include "commands.h"
11 
#include "flight.h"
 12 
#include "flight.h"
12 
#include "controlMixer.h"
 - 
 
13 
#include "rc.h"
 13 
#include "rc.h"
14 
#include "analog.h"
 14 
#include "analog.h"
15 
#include "configuration.h"
 15 
#include "configuration.h"
16 
#include "twimaster.h"
 16 
#include "controlMixer.h"
17 
#ifdef USE_NAVICTRL
 - 
 
18 
#include "spi.h"
 - 
 
19 
#endif
 - 
 
20 
#ifdef USE_MK3MAG
 - 
 
21 
#include "mk3mag.h"
 - 
 
22 
#endif
 - 
 
23 
#include "eeprom.h"
 17 
#include "eeprom.h"
- 
 
 18 
#include "printf_P.h"
Line 24... Line 19...
24 
 
 19 
 
25 
int16_t main(void) {
 20 
int16_t main(void) {
Line 26... Line 21...
26 
  uint16_t timer;
 21 
  uint16_t timer;
27 
 
 22 
 
Line 28... Line 23...
28 
  // disable interrupts global
 23 
  // disable interrupts global
29 
  cli();
 24 
  cli();
30 
 
 25 
 
Line 31... Line 26...
31 
  // analyze hardware environment
 26 
  // analyze hardware environment
32 
  CPUType = getCPUType();
 27 
  setCPUType();
33 
  BoardRelease = getBoardRelease();
 28 
  setBoardRelease();
34 
 
 29 
 
Line 35... Line -...
35 
  // disable watchdog
 - 
 
36 
  MCUSR &= ~(1 << WDRF);
 30 
  // disable watchdog
37 
  WDTCSR |= (1 << WDCE) | (1 << WDE);
 31 
  MCUSR &= ~(1 << WDRF);
38 
  WDTCSR = 0;
 - 
 
39 
 
 32 
  WDTCSR |= (1 << WDCE) | (1 << WDE);
Line 40... Line 33...
40 
  // PPM_in[CH_THROTTLE] = 0;
 33 
  WDTCSR = 0;
41 
  // Why??? They are already initialized to 0.
 34 
 
42 
  // stickPitch = stickRoll = stickYaw = 0;
 35 
// This is strange: It should NOT be necessarty to do. But the call of the same,
- 
 
 36 
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
43 
 
 37 
channelMap_default();
- 
 
 38 
 
44 
  RED_OFF;
 39 
  // initalize modules
45 
 
 40 
  output_init();
46 
  // initalize modules
 41 
  timer0_init();
47 
  output_init();
 42 
  timer2_init();
48 
  timer0_init();
 43 
  usart0_init();
49 
  usart0_Init();
 - 
 
50 
  RC_Init();
 44 
  //if (CPUType == ATMEGA644P);// usart1_Init();
51 
  analog_init();
 45 
  RC_Init();
52 
  I2C_init();
 - 
 
Line 53... Line 46...
53 
#ifdef USE_NAVICTRL
 46 
  analog_init();
54 
  SPI_MasterInit();
 47 
 
Line -... Line 48...
- 
 
 48 
  // Parameter Set handling
- 
 
 49 
  IMUConfig_readOrDefault();
- 
 
 50 
  channelMap_readOrDefault();
- 
 
 51 
  paramSet_readOrDefault();
55 
#endif
 52 
 
56 
#ifdef USE_MK3MAG
 53 
  // enable interrupts global
- 
 
 54 
  sei();
- 
 
 55 
 
Line 57... Line 56...
57 
  MK3MAG_Init();
 56 
  printf("\n\r===================================");
58 
#endif
 57 
  printf("\n\rFlightControl");
59 
 
 58 
  printf("\n\rHardware: Custom");
Line 60... Line 59...
60 
  // enable interrupts global
 59 
  printf("\n\r     CPU: Atmega644");
61 
  sei();
 60 
  if (CPUType == ATMEGA644P)
62 
 
 61 
    printf("p");
63 
  // Parameter Set handling
 62 
  printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
64 
  ParamSet_Init();
 63 
  printf("\n\r===================================");
65 
 
 64 
 
66 
  // Wait for a short time (otherwise the RC channel check won't work below)
 65 
  // Wait for a short time (otherwise the RC channel check won't work below)
67 
  // timer = SetDelay(500);
 66 
  // timer = SetDelay(500);
Line 68... Line -...
68 
  // while(!CheckDelay(timer));
 - 
 
69 
 
 - 
 
70 
  // Instead, while away the time by flashing the 2 outputs:
 67 
  // while(!CheckDelay(timer));
71 
  // First J16, then J17. Makes it easier to see which is which.
 68 
 
72 
  timer = setDelay(500);
 - 
 
73 
  OUTPUT_SET(0,1);
 - 
 
74 
  GRN_OFF;
 69 
  // Instead, while away the time by flashing the 2 outputs:
75 
  RED_ON;
 70 
  // First J16, then J17. Makes it easier to see which is which.
76 
  while (!checkDelay(timer))
 71 
  timer = setDelay(200);
77 
    ;
 - 
 
78 
 
 72 
  outputSet(0,1);
79 
  OUTPUT_SET(0,0);
 73 
  GRN_OFF;
Line 80... Line 74...
80 
 
 74 
  RED_ON;
81 
  timer = setDelay(500);
 75 
  while (!checkDelay(timer))
82 
  while (!checkDelay(timer))
 76 
    ;
- 
 
 77 
 
- 
 
 78 
  timer = setDelay(200);
Line 83... Line 79...
83 
    ;
 79 
  outputSet(0,0);
Line 84... Line 80...
84 
 
 80 
  outputSet(1,1);
- 
 
 81 
  RED_OFF;
- 
 
 82 
  GRN_ON;
Line 85... Line 83...
85 
  OUTPUT_SET(1,1);
 83 
  while (!checkDelay(timer))
86 
  RED_OFF;
 84 
    ;
87 
  GRN_ON;
 85 
 
Line 88... Line 86...
88 
  timer = setDelay(500);
 86 
  timer = setDelay(200);
- 
 
 87 
  while (!checkDelay(timer))
89 
  while (!checkDelay(timer))
 88 
    ;
90 
    ;
 89 
  outputSet(1,0);
- 
 
 90 
  GRN_OFF;
91 
 
 91 
 
92 
  timer = setDelay(500);
 92 
    printf("\n\r===================================");
Line 93... Line 93...
93 
  while (!checkDelay(timer))
 93 
 
Line 94... Line 94...
94 
    ;
 94 
#ifdef USE_NAVICTRL
- 
 
 95 
  printf("\n\rSupport for NaviCtrl");
- 
 
 96 
#endif
95 
 
 97 
 
Line 96... Line 98...
96 
  OUTPUT_SET(1,0);
 98 
#ifdef USE_DIRECT_GPS
Line 97... Line 99...
97 
 
 99 
  printf("\n\rDirect (no NaviCtrl) navigation");
98 
  beep(2000);
 100 
#endif
99 
 
 101 
 
100 
  timer = setDelay(4000);
 102 
  controlMixer_setNeutral();
- 
 
 103 
 
101 
  while (!checkDelay(timer))
 104 
  // Cal. attitude sensors and reset integrals.
102 
    ;
 105 
  attitude_setNeutral();
103 
 
 106 
 
Line 104... Line 107...
104 
  controlMixer_setNeutral();
 107 
  // Init flight parameters
- 
 
 108 
  // flight_setNeutral();
- 
 
 109 
 
- 
 
 110 
  beep(2000);
- 
 
 111 
 
- 
 
 112 
  printf("\n\rControl: ");
- 
 
 113 
  if (staticParams.bitConfig & CFG_HEADING_HOLD)
- 
 
 114 
    printf("Heading Hold");
105 
  // Cal. attitude sensors and reset integrals.
 115 
    else printf("RTL Mode");
106 
  attitude_setNeutral();
 116 
 
107 
  // Init flight parameters
 117 
  printf("\n\n\r");
108 
  flight_setNeutral();
 118 
 
109 
 
 119 
      while (1) {
110 
  // RED_OFF;
 120 
    if (runFlightControl) { // control interval
111 
 
 121 
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
Line 112... Line 122...
112 
  beep(1000);
 122 
      if (!analogDataReady) {
Line 113... Line 123...
113 
  timer2_init();
 123 
        // Analog data should have been ready but is not!!
114 
 
 124 
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
115 
  I2CTimeout = 5000;
 125 
      } else {
116 
 
 126 
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
117 
  while (1) {
 127 
 
118 
    if (runFlightControl) { // control interval
 128 
        J4HIGH;
119 
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 129 
        // This is probably the correct order:
Line 120... Line 130...
120 
      if (!analogDataReady) {
 130 
        // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
121 
        DebugOut.Digital[0] |= DEBUG_MAINLOOP_TIMER;
 131 
        // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
Line 157... Line 167...
157 
        // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
 167 
        // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
158 
        // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
 168 
        // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
159 
        SPI_TransmitByte();
 169 
        SPI_TransmitByte();
160 
      }
 170 
      }
161 
#endif
 171 
#endif
- 
 
 172 
 
- 
 
 173 
          if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
- 
 
 174 
        debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
- 
 
 175 
      } else {
- 
 
 176 
        debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
- 
 
 177 
          }      
162 
    }
 178 
    }
163 
  }
 179 
  }
164 
  return (1);
 180 
  return (1);
165 
}
 181 
}