 void pid_update() {
   // update PID & scale to power
   //print("PID_R "); print(lastShortTimeMS); print(" "); print(ctrl.rollf); print(" ");
-  switch (ctrl.navRotModeRoll) {
-    case ctrl.FLY_STAB:
-      op_roll = long(PID_roll.compute( ctrl.rollf, gyro.posX));
-      //print(gyro.posX); print(" ");
-      break;
-    case ctrl.FLY_ACRO:
-      op_roll = long(PID_roll.compute( ctrl.rollf, gyro.rateX));
-      //print(gyro.rateX); print(" ");
-      break;
-  }
+  op_roll = long(PID_roll.compute( ctrl.rollf, gyro.posX));
+  //print(gyro.posX); print(" ");
   //print(PID_roll.getError()); print(" "); print(ctrl.Kp_roll); print(" "); print(ctrl.Kp_roll * PID_roll.getError()); print(" ")
   //print(PID_roll.getIntegral()); print(" "); print(ctrl.Ki_roll); print(" "); print(ctrl.Ki_roll * PID_roll.getIntegral()); print(" ")
   //print(PID_roll.getDerivative()); print(" "); print(ctrl.Kd_roll); print(" "); print(ctrl.Kd_roll * PID_roll.getDerivative()); print(" ")
   //print(op_roll); print("\n");
 
   //print("PID_P "); print(lastShortTimeMS); print(" "); print(ctrl.pitchf); print(" ");
-  switch (ctrl.navRotModePitch) {
-    case ctrl.FLY_STAB:
-      op_pitch = long(PID_pitch.compute( ctrl.pitchf, gyro.posY));
-      //print(gyro.posY); print(" ");
-      break;
-    case ctrl.FLY_ACRO:
-      op_pitch = long(PID_pitch.compute( ctrl.pitchf, gyro.rateY));
-      //print(gyro.rateY); print(" ");
-      break;
-  }
+  op_pitch = long(PID_pitch.compute( ctrl.pitchf, gyro.posY));
+  //print(gyro.posY); print(" ");
   //print(PID_pitch.getError()); print(" "); print(ctrl.Kp_pitch); print(" "); print(ctrl.Kp_pitch * PID_pitch.getError()); print(" ")
   //print(PID_pitch.getIntegral()); print(" "); print(ctrl.Ki_pitch); print(" "); print(ctrl.Ki_pitch * PID_pitch.getIntegral()); print(" ")
   //print(PID_pitch.getDerivative()); print(" "); print(ctrl.Kd_pitch); print(" "); print(ctrl.Kd_pitch * PID_pitch.getDerivative()); print(" ")
   //print(op_pitch); print("\n");
 
   //print("PID_Y "); print(lastShortTimeMS); print(" "); print(ctrl.yawf); print(" ");
-  switch (ctrl.navRotModeYaw) {
-    case ctrl.FLY_STAB: // use angle position for "stable" mode
-      op_yaw = long(PID_yaw.compute( ctrl.yawf, gyro.posZ));
-      //print(gyro.posZ); print(" ");
-      break;
-    case ctrl.FLY_ACRO: // use angle rate for "acrobatic" mode
-      op_yaw = long(PID_yaw.compute( ctrl.yawf, gyro.rateZ));
-      //print(gyro.rateZ); print(" ");
-      break;
-  }
+  op_yaw = long(PID_yaw.compute( ctrl.yawf, gyro.posZ));
+  //print(gyro.posZ); print(" ");
   //print(PID_yaw.getError()); print(" "); print(ctrl.Kp_yaw); print(" "); print(ctrl.Kp_yaw * PID_yaw.getError()); print(" ")
   //print(PID_yaw.getIntegral()); print(" "); print(ctrl.Ki_yaw); print(" "); print(ctrl.Ki_yaw * PID_yaw.getIntegral()); print(" ")
   //print(PID_yaw.getDerivative()); print(" "); print(ctrl.Kd_yaw); print(" "); print(ctrl.Kd_yaw * PID_yaw.getDerivative()); print(" ")

  // |  yaw  | pitch
  // bl-----br

  op_throttle = ctrl.throttle;
  power_FL = op_throttle + op_pitch + op_roll - op_yaw;
  power_FR = op_throttle + op_pitch - op_roll + op_yaw;
  power_BR = op_throttle - op_pitch - op_roll - op_yaw;

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