Subversion Repositories FlightCtrl

Rev

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

Rev 2122 Rev 2124
Line 119... Line 119...
119 
        // Analog data should have been ready but is not!!
 119 
        // Analog data should have been ready but is not!!
120 
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
 120 
        debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
121 
      } else {
 121 
      } else {
122 
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
 122 
        debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
123 
          }
 123 
          }
124 
        J4HIGH;
 - 
 
125 
        // This is probably the correct order:
 124 
        // This is probably the correct order:
126 
        // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
 125 
        // The attitude computation should not depend on anything from control (except maybe the estimation of control activity level)
127 
        // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
 126 
        // The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc.
128 
        // Flight control uses results from both.
 127 
        // Flight control uses results from both.
- 
 
 128 
#ifdef DO_PROFILE
- 
 
 129 
      startProfileTimer(ATTITUDE);
- 
 
 130 
#endif
129 
        calculateFlightAttitude();
 131 
      calculateFlightAttitude();
- 
 
 132 
#ifdef DO_PROFILE
- 
 
 133 
      stopProfileTimer(ATTITUDE);
- 
 
 134 
#endif
- 
 
 135 
 
- 
 
 136 
#ifdef DO_PROFILE
- 
 
 137 
      startProfileTimer(CONTROLMIXER);
- 
 
 138 
#endif
130 
        controlMixer_periodicTask();
 139 
      controlMixer_periodicTask();
- 
 
 140 
#ifdef DO_PROFILE
- 
 
 141 
      stopProfileTimer(CONTROLMIXER);
- 
 
 142 
#endif
- 
 
 143 
 
- 
 
 144 
#ifdef DO_PROFILE
- 
 
 145 
      startProfileTimer(COMMANDS);
- 
 
 146 
#endif
131 
        commands_handleCommands();
 147 
      commands_handleCommands();
- 
 
 148 
#ifdef DO_PROFILE
- 
 
 149 
      stopProfileTimer(COMMANDS);
- 
 
 150 
#endif
- 
 
 151 
 
- 
 
 152 
#ifdef DO_PROFILE
- 
 
 153 
      startProfileTimer(FLIGHT);
- 
 
 154 
#endif
132 
        flight_control();
 155 
      flight_control();
133 
        J4LOW;
 156 
#ifdef DO_PROFILE
- 
 
 157 
      stopProfileTimer(FLIGHT);
- 
 
 158 
#endif
Line 134... Line 159...
134 
       
 159 
       
135 
        // Allow Serial Data Transmit if motors must not updated or motors are not running
 160 
        // Allow Serial Data Transmit if motors must not updated or motors are not running
136 
        if (!runFlightControl || !isFlying) {
 161 
        if (!runFlightControl || !isFlying) {
137 
          usart0_transmitTxData();
 162 
          usart0_transmitTxData();
Line 158... Line 183...
158 
 
 183 
 
159 
          if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
 184 
          if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
160 
        debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
 185 
        debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
161 
      } else {
 186 
      } else {
162 
        debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
 187 
        debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
- 
 
 188 
          }
- 
 
 189 
 
- 
 
 190 
#ifdef DO_PROFILE
- 
 
 191 
          static uint8_t profileTimer;
- 
 
 192 
          if (profileTimer++ == 0) {
- 
 
 193 
                  debugProfileTimers(24);
- 
 
 194 
          }
- 
 
 195 
#endif
163 
          }      
 196 
 
164 
  }
 197 
  }
165 
  return (1);
 198 
  return (1);