 long tmInterval;
 #endif
 
+#define LED_PIN 13
+bool blinkState = false;
+
+// MPU control/status vars
+bool dmpReady = false;  // set true if DMP init was successful
+uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+// orientation/motion vars
+Quaternion q;           // [w, x, y, z]         quaternion container
+VectorFloat gravity;    // [x, y, z]            gravity vector
+float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+
+// ================================================================
+// ===               INTERRUPT DETECTION ROUTINE                ===
+// ================================================================
+
+volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
+void dmpDataReady() {
+  mpuInterrupt = true;
+}
+
+
 /* Definição dos canais de entrada. */
-const int CH1 = 2; /* Roll (horizontal da direita) */
+const int CH1 = 8; /* Roll (horizontal da direita) */
 const int CH2 = 3; /* Pitch (vertical da direita) */
 const int CH3 = 4; /* Throttle (vertical da esquerda) */
 const int CH4 = 5; /* Yaw (horizontal da esquerda */

 * - Canal 5 (em us - entre ~1000 a ~2000)
 * - Canal 6 (em us - entre ~1000 a ~2000)
 */
void telemetry() 
{
  /* Calcula a frequência do loop */
  tmInterval = micros() - tmCheckpoint; 
  tmCheckpoint = micros();
  
  String tmValues;
  tmValues += String(rcThrottle) + ";";
  tmValues += String(rcYaw) + ";";

  motorBL.attach(MOTOR_BL);
  motorBR.attach(MOTOR_BR);

  // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  Serial.begin(115200);
  while (Serial.available() && Serial.read()); // empty buffer

  mpu.initialize();
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  mpu.setXAccelOffset(616);
  mpu.setYAccelOffset(2244);
  mpu.setZAccelOffset(300);
  mpu.setXGyroOffset(83);
  mpu.setYGyroOffset(-25);
  mpu.setZGyroOffset(44); 

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } 
  else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

  // configure LED for output
  pinMode(LED_PIN, OUTPUT);
}

/* Loop infinito do Arduino. */
void loop() {       
  // if DMP programming failed, don't try to do anything

  if (!dmpReady) {
    return;
  }

  readOrientation();

  readRC();

  //  input = ch1v;
  // myPID.Compute();
  // ch1v += (int)output;

  writeMotor();

  // D(telemetry());

  // blink LED to indicate activity
  blinkState = !blinkState;
  digitalWrite(LED_PIN, blinkState);
}

/* Efetua a leitura dos canais dos controles. */
void readRC() { 
  channels[0] = pulseIn(CH1, HIGH);
  channels[1] = pulseIn(CH2, HIGH);
  channels[2] = pulseIn(CH3, HIGH);
