Subversion Repositories FlightCtrl

Rev

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

Rev 2108 Rev 2109
Line 28... Line 28...
28 
  WDTCSR |= (1 << WDCE) | (1 << WDE);
 28 
  WDTCSR |= (1 << WDCE) | (1 << WDE);
29 
  WDTCSR = 0;
 29 
  WDTCSR = 0;
Line 30... Line 30...
30 
 
 30 
 
31 
// This is strange: It should NOT be necessarty to do. But the call of the same,
 31 
// This is strange: It should NOT be necessarty to do. But the call of the same,
32 
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
 32 
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
Line 33... Line 33...
33 
channelMap_default();
 33 
  channelMap_default();
34 
 
 34 
 
35 
  // initalize modules
 35 
  // initalize modules
36 
  output_init();
 36 
  output_init();
Line 50... Line 50...
50 
 
 50 
 
51 
  printf("\n\r===================================");
 51 
  printf("\n\r===================================");
52 
  printf("\n\rFlightControl");
 52 
  printf("\n\rFlightControl");
53 
  printf("\n\rHardware: Custom");
 53 
  printf("\n\rHardware: Custom");
- 
 
 54 
  printf("\n\r     CPU: Atmega328");
54 
  printf("\n\r     CPU: Atmega328");
 55 
  printf("\n\rSoftware: V%d.%d%c",
55 
  printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
 56 
      VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a');
Line 56... Line 57...
56 
  printf("\n\r===================================");
 57 
  printf("\n\r===================================");
57 
 
 58 
 
58 
  // Wait for a short time (otherwise the RC channel check won't work below)
 59 
  // Wait for a short time (otherwise the RC channel check won't work below)
Line 59... Line 60...
59 
  // timer = SetDelay(500);
 60 
  // timer = SetDelay(500);
60 
  // while(!CheckDelay(timer));
 61 
  // while(!CheckDelay(timer));
61 
 
 62 
 
62 
  // Instead, while away the time by flashing the 2 outputs:
 63 
  // Instead, while away the time by flashing the 2 outputs:
63 
  // First J16, then J17. Makes it easier to see which is which.
 64 
  // First J16, then J17. Makes it easier to see which is which.
64 
  timer = setDelay(200);
 65 
  timer = setDelay(200);
65 
  outputSet(0,1);
 66 
  outputSet(0, 1);
66 
  GRN_OFF;
 67 
  GRN_OFF;
Line 67... Line 68...
67 
  RED_ON;
 68 
  RED_ON;
68 
  while (!checkDelay(timer))
 69 
  while (!checkDelay(timer))
69 
    ;
 70 
    ;
70 
 
 71 
 
71 
  timer = setDelay(200);
 72 
  timer = setDelay(200);
72 
  outputSet(0,0);
 73 
  outputSet(0, 0);
73 
  outputSet(1,1);
 74 
  outputSet(1, 1);
Line 74... Line 75...
74 
  RED_OFF;
 75 
  RED_OFF;
75 
  GRN_ON;
 76 
  GRN_ON;
76 
  while (!checkDelay(timer))
 77 
  while (!checkDelay(timer))
77 
    ;
 78 
    ;
78 
 
 79 
 
Line 79... Line 80...
79 
  timer = setDelay(200);
 80 
  timer = setDelay(200);
Line 80... Line 81...
80 
  while (!checkDelay(timer))
 81 
  while (!checkDelay(timer))
81 
    ;
 82 
    ;
82 
  outputSet(1,0);
 83 
  outputSet(1, 0);
Line 102... Line 103...
102 
 
 103 
 
Line 103... Line 104...
103 
  beep(2000);
 104 
  beep(2000);
Line 104... Line 105...
104 
 
 105 
 
105 
  printf("\n\n\r");
 106 
  printf("\n\n\r");
106 
 
 107 
 
107 
      while (1) {
 108 
  while (1) {
108 
    if (runFlightControl) { // control interval
 109 
    if (runFlightControl) { // control interval
109 
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 110 
      runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
110 
      if (sensorDataReady != ALL_DATA_READY) {
 111 
      if (sensorDataReady != ALL_DATA_READY) {
111 
        // Analog data should have been ready but is not!!
 112 
        // Analog data should have been ready but is not!!
- 
 
 113 
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
- 
 
 114 
      } else {
- 
 
 115 
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
- 
 
 116 
      }
- 
 
 117 
      J4HIGH;
- 
 
 118 
      // This is probably the correct order:
- 
 
 119 
      // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
- 
 
 120 
      // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
- 
 
 121 
      // Flight control uses results from both.
- 
 
 122 
      calculateFlightAttitude();
- 
 
 123 
      controlMixer_periodicTask();
- 
 
 124 
      commands_handleCommands();
- 
 
 125 
      flight_control();
- 
 
 126 
      J4LOW;
- 
 
 127 
 
- 
 
 128 
      // Allow Serial Data Transmit if motors must not updated or motors are not running
Line 112... Line -...
112 
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
 - 
 
113 
      } else {
 - 
 
114 
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
 - 
 
115 
 
 - 
 
116 
        J4HIGH;
 - 
 
117 
        // This is probably the correct order:
 - 
 
118 
        // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
 - 
 
119 
        // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
 - 
 
120 
        // Flight control uses results from both.
 - 
 
121 
        calculateFlightAttitude();
 - 
 
122 
        controlMixer_periodicTask();
 - 
 
123 
        commands_handleCommands();
 - 
 
124 
        flight_control();
 - 
 
125 
        J4LOW;
 - 
 
126 
       
 - 
 
127 
        // Allow Serial Data Transmit if motors must not updated or motors are not running
 - 
 
128 
        if (!runFlightControl || !isFlying) {
 129 
      if (!runFlightControl || !isFlying) {
129 
          usart0_transmitTxData();
 - 
 
130 
        }
 - 
 
131 
 
 - 
 
132 
        usart0_processRxData();
 - 
 
133 
 
 - 
 
134 
        if (checkDelay(timer)) {
 - 
 
135 
          if (UBat <= UBAT_AT_5V) {
 - 
 
136 
            // Do nothing. The voltage on the input side of the regulator is <5V;
 - 
 
Line 137... Line 130...
137 
            // we must be running off USB power. Keep it quiet.
 130 
        usart0_transmitTxData();
138 
          } else if (UBat < staticParams.batteryVoltageWarning) {
 131 
      }
139 
            beepBatteryAlarm();
 132 
 
140 
          }
 - 
 
141 
 
 133 
      usart0_processRxData();
142 
#ifdef USE_NAVICTRL
 - 
 
143 
          SPI_StartTransmitPacket();
 134 
 
144 
          SendSPI = 4;
 135 
      if (checkDelay(timer)) {
Line 145... Line -...
145 
#endif
 - 
 
146 
          timer = setDelay(20); // every 20 ms
 - 
 
147 
        }
 - 
 
148 
        output_update();
 136 
        // Do nothing. The voltage on the input side of the regulator is <5V;
149 
      }
 - 
 
150 
 
 - 
 
151 
#ifdef USE_NAVICTRL
 137 
        // we must be running off USB power. Keep it quiet.
152 
      if(!SendSPI) {
 138 
      } else if (UBat < staticParams.batteryVoltageWarning && UBat > UBAT_AT_5V) {
153 
        // SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
 - 
 
154 
        // within the SPI_TransmitByte() routine the value is set to 4.
 - 
 
Line -... Line 139...
- 
 
 139 
        beepBatteryAlarm();
- 
 
 140 
      }
155 
        // I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
 141 
 
156 
        // and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
 142 
      timer = setDelay(20); // every 20 ms
157 
        SPI_TransmitByte();
 143 
      output_update();
158 
      }
 144 
    }
159 
#endif
 - 
 
160 
          calculateFeaturedServoValues();
 145 
 
161 
 
 146 
    calculateFeaturedServoValues();
162 
          if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
 147 
 
163 
        debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
 148 
    if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.