     /*if(ctrl.live){
         logger.append_IMU(lastloggerTime, gyro.posX, gyro.posY, gyro.posZ);
         logger.append_PID(lastloggerTime, op_roll, op_pitch, op_yaw);
-        logger.append_MP(lastloggerTime, power_FL, power_FR, power_BR, power_BL);
+        //logger.append_MP(lastloggerTime, power_FL, power_FR, power_BR, power_BL);
         logger.append_RX(lastloggerTime, ctrl.rollf, ctrl.pitchf, ctrl.yawf, ctrl.throttle);
         logger.writeOnFull();        
     }*/
-    print("IMU "); print(lastShortTimeMS); print(" "); print(gyro.posX); print(" "); print(gyro.posY); print(" "); print(gyro.posZ); print("\n");
-    //print("GRATE "); print(lastShortTimeMS); print(" "); print(gyro.rateX); print(" "); print(gyro.rateY); print(" "); print(gyro.rateZ); print("\n");
-    print("GRSUM "); print(lastShortTimeMS); print(" "); print(gyro.rsumX); print(" "); print(gyro.rsumY); print(" "); print(gyro.rsumZ); print("\n");
-    print("ACC "); print(lastShortTimeMS); print(" "); print(accel.x); print(" "); print(accel.y); print(" "); print(compass.yaw); print("\n");
-    //print("MAG "); print(lastShortTimeMS); print(" ");print(compass.mx); print(" "); print(compass.my); print(" "); print(compass.mz); print("\n");
-    print("PID "); print(lastShortTimeMS); print(" "); print(op_roll); print(" "); print(op_pitch); print(" "); print(op_yaw); print("\n");
-    print("MP "); print(lastShortTimeMS); print(" "); print(power_FL); print(" "); print(power_FR); print(" "); print(power_BR); print(" "); print(power_BL); print("\n");
+    #if DEBUG
+        print("IMU "); print(lastShortTimeMS); print(" "); print(gyro.posX); print(" "); print(gyro.posY); print(" "); print(gyro.posZ); print("\n");
+        //print("GRATE "); print(lastShortTimeMS); print(" "); print(gyro.rateX); print(" "); print(gyro.rateY); print(" "); print(gyro.rateZ); print("\n");
+        //print("GRSUM "); print(lastShortTimeMS); print(" "); print(gyro.rsumX); print(" "); print(gyro.rsumY); print(" "); print(gyro.rsumZ); print("\n");
+        //print("ACC "); print(lastShortTimeMS); print(" "); print(accel.x); print(" "); print(accel.y); print(" "); print(compass.yaw); print("\n");
+        //print("MAG "); print(lastShortTimeMS); print(" ");print(compass.mx); print(" "); print(compass.my); print(" "); print(compass.mz); print("\n");
+        print("PID "); print(lastShortTimeMS); print(" "); print(op_roll); print(" "); print(op_pitch); print(" "); print(op_yaw); print("\n");
+        print("MP "); print(lastShortTimeMS); print(" "); print(power_FL); print(" "); print(power_FR); print(" "); print(power_BR); print(" "); print(power_BL); print("\n");
+    #endif
   }
 }
 

    Yscale = longCord/(Ymax - Ymin);
    Zscale = longCord/(Zmax - Zmin);

    #ifdef DEBUG
        Serial.printf("X min %i max %i = offset %0.6f scale %0.6f\n", Xmin, Xmax, Xoffset, Xscale);
        Serial.printf("Y min %i max %i = offset %0.6f scale %0.6f\n", Ymin, Ymax, Yoffset, Yscale);
        Serial.printf("Z min %i max %i = offset %0.6f scale %0.6f\n", Zmin, Zmax, Zoffset, Zscale);
    #endif
  }
  compass.config(Xoffset,Yoffset,Zoffset,Xscale,Yscale,Zscale);

}

void motor_off() {
    power_FL = 0; 
    power_FR = 0; 
    power_BR = 0; 
    power_BL = 0;
    esc.update(motorPin_FL, 0);
    esc.update(motorPin_FR, 0);
    esc.update(motorPin_BR, 0);
