const int echoPin = PIN_PA08;
const int triggerPin = PIN_PA14;

int previous_cm = 0;
int outlier_threshold = 100;
int out_of_bounds = 200;

void setup() {
   Serial.begin(9600);
   Serial1.begin(9600);
   pinMode(triggerPin, OUTPUT);
   pinMode(echoPin, INPUT);
}

void loop() {
   delay(20);
   int cm = ping(triggerPin, echoPin);
   processDistance(cm);
}

int ping(int TriggerPin, int EchoPin) {
   long duration;
   long distanceCm;

   digitalWrite(TriggerPin, LOW);  
   delayMicroseconds(4);
   digitalWrite(TriggerPin, HIGH); 
   delayMicroseconds(10);
   digitalWrite(TriggerPin, LOW);

   duration = pulseIn(EchoPin, HIGH); 
   distanceCm = duration * 10 / 292 / 2; 

   return distanceCm;
}

void processDistance(int cm) {  
  if((cm < out_of_bounds && abs(previous_cm - cm) < outlier_threshold) || previous_cm == 0){
    Serial.print(cm);
    Serial.print('\n');
    Serial1.print(cm);
    Serial1.print('\n');
    previous_cm = cm;
  } else { 
    Serial.println("Discarding measurment due to outlier detection."); 
  }
}