     D(Serial.print(dmpStatus));
     D(Serial.println(F(")")));
   }
-
-  // configure LED for output
-  pinMode(LED_PIN, OUTPUT);
-}
-
-/* Loop infinito do Arduino. */
-void loop() {
-  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();
-
-    // 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
-    }
-
-    // pid_pitch_rate.calculate()
-    // pid_roll_rate.calculate()
-    // pid_yaw_rate.calculate()
-
-    //  // Stablise PIDS                                                     'setpoint' 'input'
-    // float pitch_stab_output = constrain(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250);
-    // float roll_stab_output = constrain(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250);
-    // float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360);
-
-    // // 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 = yaw;   // remember this yaw for when pilot stops
-    // }
-
-    // // rate PIDS                                                       'setpoint'         'input'
-    // long pitch_output =  (long) constrain(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500, 500);
-    // long roll_output =  (long) constrain(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500);
-    // long yaw_output =  (long) constrain(pids[PID_YAW_RATE].get_pid(yaw_stab_output - gyroYaw, 1), -500, 500);
-
-    // // mix pid outputs and send to the motors.
-    // hal.rcout->write(MOTOR_FL, rcthr + roll_output + pitch_output - yaw_output);
-    // hal.rcout->write(MOTOR_BL, rcthr + roll_output - pitch_output + yaw_output);
-    // hal.rcout->write(MOTOR_FR, rcthr - roll_output + pitch_output + yaw_output);
-    // hal.rcout->write(MOTOR_BR, rcthr - roll_output - pitch_output - yaw_output);
-
-    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;
-
+}
+
+/* Define os parâmetros dos PIDs com base no dimmer do controle. O range possível é de 0 a 25.5.
+ * Atualmente, os valores do PID são compartilhados entre o roll, pitch e yaw.
+ * O parâmetro sendo calibrado é definido de forma hard-coded. */
+void setPIDParameters(char param) {
+  if (param == 'P') {
+    pidParams.PIDPitchStab.kp = pidParams.PIDRollStab.kp = pidParams.PIDYawStab.kp = rcDimmer/10;
+  } else if (param == 'I') {
+    pidParams.PIDPitchStab.ki = pidParams.PIDRollStab.ki = pidParams.PIDYawStab.ki = rcDimmer/10;
   } 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];
-
-    // reset PID integrals whilst on the ground
-    // for(int i=0; i<6; i++)
-    //   pids[i].reset_I();
-
-    // }
-  }
-
-  // D(Serial.println("Wrt mtr"));
-  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); /* Roll (horizontal da direita) */
-  channels[1] = pulseIn(CH2, HIGH); /* Pitch (vertical da direita) */
-  channels[2] = pulseIn(CH3, HIGH); /* Throttle (vertical da esquerda) */
-  channels[3] = pulseIn(CH4, HIGH); /* Yaw (horizontal da esquerda */
-  channels[4] = pulseIn(CH5, HIGH); /* ON/OFF */
-  channels[5] = pulseIn(CH6, HIGH); /* Dimmer */
-
-  normalizeRC();
+    pidParams.PIDPitchStab.kd = pidParams.PIDRollStab.kd = pidParams.PIDYawStab.kd = rcDimmer/10;
+  }
+
+  PIDPitchStab.SetTunings(pidParams.PIDPitchStab.kp, pidParams.PIDPitchStab.ki, pidParams.PIDPitchStab.kd);
+  PIDRollStab.SetTunings(pidParams.PIDRollStab.kp, pidParams.PIDRollStab.ki, pidParams.PIDRollStab.kd);
+  PIDYawStab.SetTunings(pidParams.PIDYawStab.kp, pidParams.PIDYawStab.ki, pidParams.PIDYawStab.kd);
+
+  pidChanged = true;
+}
+
+/* Escreve os parâmetros do PID na EEPROM */
+void writePIDParameters() {
+  EEPROM.write(EEPROM_PITCH_P, (int)(pidParams.PIDPitchStab.kp * 10));
+  EEPROM.write(EEPROM_PITCH_I, (int)(pidParams.PIDPitchStab.ki * 10));
+  EEPROM.write(EEPROM_PITCH_D, (int)(pidParams.PIDPitchStab.kd * 10));
+
+  EEPROM.write(EEPROM_ROLL_P, (int)(pidParams.PIDRollStab.kp * 10));
+  EEPROM.write(EEPROM_ROLL_I, (int)(pidParams.PIDRollStab.ki * 10));
+  EEPROM.write(EEPROM_ROLL_D, (int)(pidParams.PIDRollStab.kd * 10));
+
+  EEPROM.write(EEPROM_YAW_P, (int)(pidParams.PIDYawStab.kp * 10));
+  EEPROM.write(EEPROM_YAW_I, (int)(pidParams.PIDYawStab.ki * 10));
+  EEPROM.write(EEPROM_YAW_D, (int)(pidParams.PIDYawStab.kd * 10));
+}
+
+/* Efetua a calibração se a calibração estiver ativa (controle ON/OFF ligado) e grava a EEPROM ao final da calibração. Escolha do parâmetro de forma hard-coded. */
+void PIDCalibration() {
+  if (rcOnOff > 10) {
+    setPIDParameters('P');
+  } else if (pidChanged) {
+    D(Serial.println("PIDs gravados na EEPROM"));
+    writePIDParameters();
+    pidChanged = false;
+  }
 }
 
 /* Normaliza os valores do controle para ranges pré definidos. */
 void normalizeRC() {
-  rcRoll     = map(channels[0], MIN_RC_ROLL, MAX_RC_ROLL, -45, 45);
-  rcPitch    = map(channels[1], MIN_RC_PITCH, MAX_RC_PITCH, 45, -45);
+
+  /* Leva em consideração o zero do roll controle */
+  if (rcRoll > rcRollZero) {
+    rcRoll = map(channels[0], rcRollZero, MAX_RC_ROLL, 0, 45);
+  } else {
+    rcRoll = map(channels[0], MIN_RC_ROLL, rcRollZero, -45, 0);
+  }
+
+  /* Leva em consideração o zero do roll controle */
+  if (rcPitch > rcRollZero) {
+    rcPitch = map(channels[1], rcPitchZero, MAX_RC_PITCH, 0, 45);
+  } else {
+    rcPitch = map(channels[1], MIN_RC_PITCH, rcPitchZero, -45, 0);
+  }
+
+  /* Leva em consideração o zero do yaw controle */
+  if (rcYaw > rcYawZero) {
+    rcYaw = map(channels[3], rcYawZero, MAX_RC_YAW, 0, 150);
+  } else {
+    rcYaw = map(channels[3], MIN_RC_YAW, rcYawZero, -150, 0);
+  }
+
   rcThrottle = channels[2];
-  rcYaw      = map(channels[3], MIN_RC_YAW, MAX_RC_YAW, -150, 150);
   rcOnOff    = map(channels[4], MIN_RC_ON_OFF, MAX_RC_ON_OFF, 0, 100);
-  rcDimmer   = map(channels[5], MIN_RC_DIMMER, MAX_RC_DIMMER, 0, 1000);
+  rcDimmer   = map(channels[5], MIN_RC_DIMMER, MAX_RC_DIMMER, 0, 255);
+}
+
+/* Define os valores referentes ao zero do controle para Roll, Yaw e Pitch com base em 10 leituras do controle. */
+void getZeroValuesFromRc() {
+
+  /* Roll (horizontal da direita) */
+  for (int i = 0; i < 10; i++) {
+    rcRollZero += pulseIn(CH1, HIGH, 20000);
+  }
+  rcRollZero /= 10;
+
+  /* Pitch (vertical da direita) */
+  for (int i = 0; i < 10; i++) {
+    rcPitchZero += pulseIn(CH2, HIGH, 20000);
+  }
+  rcPitchZero /= 10;
+
+  /* Yaw (horizontal da esquerda */
+  for (int i = 0; i < 10; i++) {
+    rcYawZero += pulseIn(CH4, HIGH, 20000);
+  }
+  rcYawZero /= 10;
 }
 
 /* Efetua a escrita da potência dos motores entre 1ms e 2ms */

const int DMP_PITCH = 1;
const int DMP_ROLL = 2;


/****************
 * Interrupções *
 ****************/

  }
}

/* Cria padding em uma string preenchendo-a com espaços à direita */
String padding(String text, int size){
  String newString = String(text);
  if (text.length() < size) {
