   D(Serial.begin(115200));
   D(while (Serial.available() && Serial.read())); // Limpa o buffer
 
+  D(Serial.println("Iniciando controle remoto"));
+  setupRC();
+
+  D(Serial.println("Iniciando motores"));
+  setupMotors();
+
+  D(Serial.println("Iniciando PID"));
+  setupPID();
+
+  D(Serial.println("Iniciando MPU"));
+  setupMPU();
+
+  /* Configura a led para output de atividade */
+  pinMode(LED_PIN, OUTPUT);
+
+  D(Serial.println("Fim do setup"));
+}
+
+/* Loop infinito do Arduino. */
+void loop() {
+  static float yaw_target = 0;
+
+  /* Se a programação do DMP falhou, não faz nada */
+  if (!dmpReady) {
+    return;
+  }
+
+  readOrientation();
+
+  readRC();
+
+  PIDCalibration();
+
+  /* Voe Forest, Voe! */
+  if(rcThrottle > MIN_RC_THROTTLE + 100) {  // Throttle raised, turn on stablisation.
+    PIDPitchStab.Compute();
+    PIDRollStab.Compute();
+    PIDYawStab.Compute();
+
+    // is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
+    if(abs(rcYaw ) > 5) {
+      yaw_stab_output = rcYaw;
+      yaw_target = ypr_degree[DMP_YAW];   // remember this yaw for when pilot stops
+    }
+
+    motors[MOTOR_FL] = rcThrottle + roll_stab_output + pitch_stab_output - yaw_stab_output;
+    motors[MOTOR_BL] = rcThrottle + roll_stab_output - pitch_stab_output + yaw_stab_output;
+    motors[MOTOR_FR] = rcThrottle - roll_stab_output + pitch_stab_output + yaw_stab_output;
+    motors[MOTOR_BR] = rcThrottle - roll_stab_output - pitch_stab_output - yaw_stab_output;
+
+  } else {
+    /* Motores desligados */
+    motors[MOTOR_FL] = motors[MOTOR_FR] = motors[MOTOR_BL] = motors[MOTOR_BR] = 1000;
+
+    // reset yaw target so we maintain this on takeoff
+    yaw_target = ypr_degree[DMP_YAW];
+  }
+
+  writeMotor();
+
+  D(telemetry());
+
+  /* Pisca a led para indicar atividade */
+  blinkState = !blinkState;
+  digitalWrite(LED_PIN, blinkState);
+}
+
+/* Efetua a leitura dos canais dos controles. */
+void readRC() {
+  channels[0] = pulseIn(CH1, HIGH, 20000); /* Roll (horizontal da direita) */
+  channels[1] = pulseIn(CH2, HIGH, 20000); /* Pitch (vertical da direita) */
+  channels[2] = pulseIn(CH3, HIGH, 20000); /* Throttle (vertical da esquerda) */
+  channels[3] = pulseIn(CH4, HIGH, 20000); /* Yaw (horizontal da esquerda */
+  channels[4] = pulseIn(CH5, HIGH, 20000); /* ON/OFF */
+  channels[5] = pulseIn(CH6, HIGH, 20000); /* Dimmer */
+
+  normalizeRC();
+}
+
+/* Obtem os valores dos PIDs da EEPROM */
+void getPIDParametersAndSetTunnings() {
+  D(char tempString[10]);
+
+  pidParams.PIDPitchStab.kp = (float)EEPROM.read(EEPROM_PITCH_P) / 10;
+  pidParams.PIDPitchStab.ki = (float)EEPROM.read(EEPROM_PITCH_I) / 10;
+  pidParams.PIDPitchStab.kd = (float)EEPROM.read(EEPROM_PITCH_D) / 10;
+  PIDPitchStab.SetTunings(pidParams.PIDPitchStab.kp, pidParams.PIDPitchStab.ki, pidParams.PIDPitchStab.kd);
+
+  pidParams.PIDRollStab.kp = (float)EEPROM.read(EEPROM_ROLL_P) / 10;
+  pidParams.PIDRollStab.ki = (float)EEPROM.read(EEPROM_ROLL_I) / 10;
+  pidParams.PIDRollStab.kd = (float)EEPROM.read(EEPROM_ROLL_D) / 10;
+  PIDRollStab.SetTunings(pidParams.PIDRollStab.kp, pidParams.PIDRollStab.ki, pidParams.PIDRollStab.kd);
+
+
+  pidParams.PIDYawStab.kp = (float)EEPROM.read(EEPROM_YAW_P) / 10;
+  pidParams.PIDYawStab.ki = (float)EEPROM.read(EEPROM_YAW_I) / 10;
+  pidParams.PIDYawStab.kd = (float)EEPROM.read(EEPROM_YAW_D) / 10;
+  PIDYawStab.SetTunings(pidParams.PIDYawStab.kp, pidParams.PIDYawStab.ki, pidParams.PIDYawStab.kd);
+
+  D(Serial.print("Valores do PID: ("));
+  D(dtostrf(pidParams.PIDPitchStab.kp, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDPitchStab.ki, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDPitchStab.kd, 2, 1, tempString));
+  D(Serial.print(String(tempString) + "), ("));
+  D(dtostrf(pidParams.PIDRollStab.kp, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDRollStab.ki, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDRollStab.kd, 2, 1, tempString));
+  D(Serial.print(String(tempString) + "), ("));
+  D(dtostrf(pidParams.PIDYawStab.kp, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDYawStab.ki, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDYawStab.kd, 2, 1, tempString));
+  D(Serial.println(String(tempString) + ")"));
+}
+
+/* Efetua o setup do controle remoto */
+void setupRC() {
   /* Bind dos canais de entrada do controle */
   pinMode(CH1, INPUT);
   pinMode(CH2, INPUT);

/* 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) {

  tmValues += padding("MBL=" + String(int(motors[MOTOR_BL])), 8) + ";";
  tmValues += padding("MBR=" + String(int(motors[MOTOR_BR])), 8) + ";";
  tmValues += padding("INT=" + String(int(tmInterval)), 10) + ";";
  // for (int i=0; i < 6; i++) {
  //  tmValues += padding(String(channels[i]), 8) + ";";
  // }

  countToSend++;
  if (countToSend > 4) {
