 /* Efetua a leitura da orientação a ser armazenada na variável "ypr" */
 void readOrientation() {
   bool fifoOverflow = false; // Marca se houve overflow da FIFO durante a execução
+  D(Serial.println("init RO"));
 
   /* Le somente se houve interrupção */
   if (mpuInterrupt) {


    /* Obtém o byte INT_STATUS do MPU */
    mpuIntStatus = mpu.getIntStatus();

    /* Obtém a contagem da FIFO */
    fifoCount = mpu.getFIFOCount();

    /* Verifica se houve overflow da FIFO. Quanto mais ineficiente o código, mais irá ocorrer. */
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
      // Reseta a FIFO
      mpu.resetFIFO();
      fifoOverflow = true;
    }

    /* Se houve overflow, aguarda o próximo valor estar pronto */
    if (fifoOverflow) {
      while(!mpuInterrupt);
    }

    if (mpuIntStatus & 0x02) {
      /* Obtém a contagem da FIFO */
      fifoCount = mpu.getFIFOCount();

      /* Aguarda encher a FIFO caso não esteja com os todos dados completos (pacote completo) */
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

      /* Dado está pronto para ser lido do MPU */
      mpu.getFIFOBytes(fifoBuffer, packetSize);

      /* Obtém os valores (Yaw, Pitch e Roll) do DMP */
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

      /* Converte para graus/segundo */
      for (int i = 0; i < 3; i++) {
        ypr_degree[i] = ypr[i] * 180/M_PI;
      }
    }
  }

  static float yaw_target = 0;

  /* Se a programação do DMP falhou, não faz nada */
  // D(Serial.println("DMP rdy"));
  if (!dmpReady) {
    return;
  }

  D(Serial.println(" "));
  D(Serial.println("Red Ori"));
  readOrientation();

  // D(Serial.println("Read RC"));
  readRC();

  /* Voe Forest, Voe! */
  if(rcThrottle > MIN_RC_THROTTLE + 100) {  // Throttle raised, turn on stablisation.
    pid_pitch_stab.Compute();
    pid_roll_stab.Compute();
    pid_yaw_stab.Compute();
