121,16 → 121,41 |
} else { |
debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
} |
J4HIGH; |
// This is probably the correct order: |
// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level) |
// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc. |
// Flight control uses results from both. |
#ifdef DO_PROFILE |
startProfileTimer(ATTITUDE); |
#endif |
calculateFlightAttitude(); |
#ifdef DO_PROFILE |
stopProfileTimer(ATTITUDE); |
#endif |
|
#ifdef DO_PROFILE |
startProfileTimer(CONTROLMIXER); |
#endif |
controlMixer_periodicTask(); |
#ifdef DO_PROFILE |
stopProfileTimer(CONTROLMIXER); |
#endif |
|
#ifdef DO_PROFILE |
startProfileTimer(COMMANDS); |
#endif |
commands_handleCommands(); |
#ifdef DO_PROFILE |
stopProfileTimer(COMMANDS); |
#endif |
|
#ifdef DO_PROFILE |
startProfileTimer(FLIGHT); |
#endif |
flight_control(); |
J4LOW; |
#ifdef DO_PROFILE |
stopProfileTimer(FLIGHT); |
#endif |
|
// Allow Serial Data Transmit if motors must not updated or motors are not running |
if (!runFlightControl || !isFlying) { |
161,6 → 186,14 |
} else { |
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
} |
|
#ifdef DO_PROFILE |
static uint8_t profileTimer; |
if (profileTimer++ == 0) { |
debugProfileTimers(24); |
} |
#endif |
|
} |
return (1); |
} |