Ler da FastSerial o GPS e Barometer e do Pino 2 e 3 o Encoder a 2000Khz

GPS_Encoder_Barometer_Reader.cpp

FastSerialPort0(Serial);
FastSerialPort2(serialGPS);
FastSerialPort1(serialIMU);

Encoder encoder(2,3);

void loop(){

  Serial.println(readGPS(true));
  Serial.println(readIMU());
  Serial.println(readEncoder());

  delay(100);
}

String readGPS(boolean segunda){

  return readLine(serialGPS, segunda);
}

String readIMU(){

  String str = String("%delfino,3050,");
  return str + readLine(serialIMU, false);
}

String readEncoder(){

  long newPosition = encoder.read();
  changePosition = newPosition != oldPosition;
  if (changePosition) {
    oldPosition = newPosition;
  }
  String str = String("%delfino,4050,");

  str += oldPosition;
  str += ",";
    str += newPosition;
  return str;
}

String readLine(FastSerial serial, boolean segunda){

  String data;

  char buffer1[0xFF] ;
  char buffer2[0xFF] ;
  int read1 = serial.readBytesUntil((char)0x0D,buffer1,0xFF)-1;

  int read2;
  if(segunda)read2 = serial.readBytesUntil((char)0x0D,buffer2,0xFF)-1;

  for(int i = 0; i < read1; i++){
    if(buffer1[i] == 0x0A)continue;
    data += buffer1[i];
  }
  if(segunda){    
    data += "\r\n";
    for(int j = 0; j < read2; j++){
      if(buffer2[j] == 0x0A)continue;
      data += buffer2[j];
    }
  }

  return data;
}