 }
 
 void rcInterrupt1(){
-   ch1 = micros() - rcLastChange1;
+   if(!interruptLock) ch1 = micros() - rcLastChange1;
    rcLastChange1 = micros(); 
 }
 
 void rcInterrupt2(){
-  ch2 = micros() - rcLastChange2;
+  if(!interruptLock) ch2 = micros() - rcLastChange2;
   rcLastChange2 = micros();
 }
 
 void rcInterrupt3(){
-  ch3 = micros() - rcLastChange3;
+  if(!interruptLock) ch3 = micros() - rcLastChange3;
   rcLastChange3 = micros();
 }
 
 void rcInterrupt4(){
-  ch4 = micros() - rcLastChange4;
+  if(!interruptLock) ch4 = micros() - rcLastChange4;
   rcLastChange4 = micros();
 }
 
 void rcInterrupt5(){
-  ch5 = micros() - rcLastChange5;
+  if(!interruptLock) ch5 = micros() - rcLastChange5;
   rcLastChange5 = micros();
 }
 
+void acquireLock(){
+  interruptLock = true; 
+}
+
+void releaseLock(){
+  interruptLock = false;
+}
+
 #endif
 
 

#define ROLL_MAX 30
#define YAW_MIN -180
#define YAW_MAX 180
#define PID_PITCH_INFLUENCE 50
#define PID_ROLL_INFLUENCE 50
#define PID_YAW_INFLUENCE 20



  ypr[0] = ypr[0] * 180/M_PI;
  ypr[1] = ypr[1] * 180/M_PI;
  ypr[2] = ypr[2] * 180/M_PI;
  
  if(abs(ypr[0]-yprLast[0])>30) ypr[0] = yprLast[0];
  if(abs(ypr[1]-yprLast[1])>30) ypr[1] = yprLast[1];
  if(abs(ypr[2]-yprLast[2])>30) ypr[2] = yprLast[2];
  
  yprLast[0] = ypr[0];
  yprLast[1] = ypr[1];
  yprLast[2] = ypr[2];

  pitchReg.Compute();
  rollReg.Compute();
  yawReg.Compute();
  
  releaseLock();

}

