hey guys, 

wasn't quite sure where to post this so i'm putting it here : please refer me if i misplaced this post.

im building  a quadrotor on my own from scratch. now- im at a quite advanced state.

my problem is software related mostly. my quad is also arduino based and has a arduinoMega2560 at its heart. the sensors(IMU) i use are the ones on the 9dof sensor stick by sparkfun - (http://www.sparkfun.com/products/10724), communication is done by i2c. here comes the problem:

from what i understand the PID control loop needs to be running at at least 100Hz, more so i read that the frequency stability is also really important.  right now my problem is that my control isn't tight enough and doesn't respond quickly enough to my inputs. i think the cause to the problem is the fact that my control loop is running at ~150Hz but very unstable. (by that i mean lots of bouncing in the frequencies)

in my program i have 2 loops each have a separate timer setup for them: the Control loop and the Coms loop. the control loop does the following: read the sensors (i2c),read the RC remote,calculate PID,update motor. the Coms loop handles communication with the ground station computer. the 2 timers i set up each have a separate handler each setting a flag and then the main loop looks something like:

while(1)

  if(ControlLoopFlag)  -> execute control loop, lower the flag.

  if(ComsLoopFlag)    -> execute Coms Loop, lower the flag.

at the end of each loop if the execution time is slower than a certain number then i wait. (to prevent spikes in the frequency). i was forced to adapt this design because the Arduino framework in which i work doesn't allow nested interrupts meaning i cant read the i2c sensors and handle serial info in an interrupt handler( which is what i would like to do : put the whole loop in a ISR thus causing a steady frequency). i also tried enabling the interrupts inside the ISR and then reading the sensors but that crashed the controller. if such a thing was possible i would be able to stick the whole loops inside separate ISR's and solve the problem. (if u still dont understand then ill try to explain. since the control loop isnt inside an ISR (only the flag setting is) i cant stop the com loop in the middle thus causing unstable run frequency on the control loop. the same goes for the com loop)

my question to you guys is what design do i need to use(or how to modify mine) in order to ensure a stable frequency?

thanks in advance for all the comments, and if u have any questions regarding my system please ask.