   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 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;
    


void loop(){
  SCMD.readSerial();
	Gyro_getADC(); // read raw gyro data
	ACC_getADC(); // read raw accel data
  imu.calculate(gyroADC,accADC,&pitchAngle,&rollAngle);


  out = out / 4096 / 8;

  return out; 
}

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