   int32_t pitchP = pitchPID[0];
   int32_t pitchI = pitchPID[1];
   int32_t pitchD = pitchPID[2];
-  
+ 
   int32_t rollP = rollPID[0];
   int32_t rollI = rollPID[1];
   int32_t rollD = rollPID[2];

  subTick++;
}

int32_t ComputePID(int32_t DTms, int32_t DTinv, int32_t in, int32_t setPoint, int32_t *errorSum, int32_t *errorOld, int32_t Kp, int16_t Ki, int32_t Kd)
{
  int32_t error = setPoint - in;
  int32_t Ierr;
   


  out = out / 4096 / 8;

  return out; 
}

inline int32_t constrain_int32(int32_t x , int32_t l, int32_t h){
