private void WriteFrameToOutputPorts(Frame aFrame) {
// Write frame with valid data
WriteIntegerToFilterOutputPort(TIMESTAMP_ID, FilterFramework.outPipe1);
WriteLongToFilterOutputPort(aFrame.getTimeStamp(), FilterFramework.outPipe1);
WriteIntegerToFilterOutputPort(VELOCITY_ID, FilterFramework.outPipe1);
WriteLongToFilterOutputPort(aFrame.getVelocity(), FilterFramework.outPipe1);
WriteIntegerToFilterOutputPort(ALTITUDE_MEASURE_ID, FilterFramework.outPipe1);
WriteLongToFilterOutputPort(aFrame.getAltitudeMeasure(), FilterFramework.outPipe1);
if (aFrame.getHasInvalidPressureMeasure() == true) {
WriteIntegerToFilterOutputPort(PRESSURE_EXTRAPOLATED_MEASURE_ID, FilterFramework.outPipe1);
WriteLongToFilterOutputPort(
aFrame.getExtrapolatedPressureMeasure(), FilterFramework.outPipe1);
} else {
WriteIntegerToFilterOutputPort(PRESSURE_MEASURE_ID, FilterFramework.outPipe1);
WriteLongToFilterOutputPort(aFrame.getPressureMeasure(), FilterFramework.outPipe1);
}
WriteIntegerToFilterOutputPort(TEMPERATURE_MEASURE_ID, FilterFramework.outPipe1);
WriteLongToFilterOutputPort(aFrame.getTemperatureMeasure(), FilterFramework.outPipe1);
WriteIntegerToFilterOutputPort(ATTITUDE_MEASURE_ID, FilterFramework.outPipe1);
WriteLongToFilterOutputPort(aFrame.getAttitudeMeasure(), FilterFramework.outPipe1);
// Write wildpoints
if (aFrame.getHasInvalidPressureMeasure() == true) {
WriteIntegerToFilterOutputPort(TIMESTAMP_ID, FilterFramework.outPipe2);
WriteLongToFilterOutputPort(aFrame.getTimeStamp(), FilterFramework.outPipe2);
WriteIntegerToFilterOutputPort(PRESSURE_MEASURE_ID, FilterFramework.outPipe2);
WriteLongToFilterOutputPort(aFrame.getPressureMeasure(), FilterFramework.outPipe2);
}
}
// Tries to extrapolate and fill extrapolated pressure measures to
// frames with invalid pressure
private void ExtrapolateFramesInList(List<Frame> aFrameQueue, boolean aIsOnFlush) {
if (aFrameQueue.isEmpty()) {
return;
}
Frame firstFrame = aFrameQueue.get(0);
if (firstFrame.getHasInvalidPressureMeasure() == true
&& firstFrame.getExtrapolatedPressureMeasure() == 0) {
// the stream has started with invalid pressure measure.
// Find last valid and use it to fill invalid measurements
long validMeasure = 0;
Frame fr = null;
for (int i = 0; i < aFrameQueue.size(); i++) {
fr = aFrameQueue.get(i);
if ((fr.getHasInvalidPressureMeasure() == false)
|| (fr.getHasInvalidPressureMeasure() == true
&& fr.getExtrapolatedPressureMeasure() > 0)) {
validMeasure = fr.getPressureMeasure();
break;
}
}
for (int i = 0; i < aFrameQueue.size(); i++) {
fr = aFrameQueue.get(i);
fr.setExtrapolatedPressureMeasure(validMeasure);
aFrameQueue.set(i, fr);
if (fr.getHasInvalidPressureMeasure() == false) {
break;
}
}
} else {
if (aFrameQueue.size() < 3) {
if (aIsOnFlush && aFrameQueue.size() > 1) {
// in that case there are 2 elements.
// First inserted is valid
// Second is invalid
// In that no third element is expeced. Use first one to fill the pressure of second one
Frame fr = aFrameQueue.get(0);
long validPressure =
fr.getHasInvalidPressureMeasure()
? fr.getExtrapolatedPressureMeasure()
: fr.getPressureMeasure();
fr = aFrameQueue.get(1);
fr.setExtrapolatedPressureMeasure(validPressure);
aFrameQueue.set(1, fr);
} else {
// in that case there are 2 elements.
// First inserted is valid
// Second is invalid
// In that case do nothing. Thirt element required for decition how to fix invalid
// pressure
return;
}
} else {
// Got 3 ot more elements
// and the first one is valid
Frame fr = aFrameQueue.get(1);
boolean isValidSecond =
(fr.getHasInvalidPressureMeasure() == false
|| (fr.getHasInvalidPressureMeasure() == true
&& fr.getExtrapolatedPressureMeasure() > 0));
fr = aFrameQueue.get(2);
boolean isValidThird =
(fr.getHasInvalidPressureMeasure() == false
|| (fr.getHasInvalidPressureMeasure() == true
&& fr.getExtrapolatedPressureMeasure() > 0));
if (!isValidSecond && !isValidThird) {
// Extrapolate the second with the first value
fr = aFrameQueue.get(0);
long validPressure =
fr.getHasInvalidPressureMeasure()
? fr.getExtrapolatedPressureMeasure()
: fr.getPressureMeasure();
// fix second with right pressure
fr = aFrameQueue.get(1);
fr.setExtrapolatedPressureMeasure(validPressure);
aFrameQueue.set(1, fr);
}
if (!isValidSecond && isValidThird) {
// Interpolate second value
fr = aFrameQueue.get(0);
long firstValidPressure =
fr.getHasInvalidPressureMeasure()
? fr.getExtrapolatedPressureMeasure()
: fr.getPressureMeasure();
fr = aFrameQueue.get(2);
long secondValidPressure =
fr.getHasInvalidPressureMeasure()
? fr.getExtrapolatedPressureMeasure()
: fr.getPressureMeasure();
double interpolVal = (firstValidPressure + secondValidPressure) / 2.0;
fr = aFrameQueue.get(1);
fr.setExtrapolatedPressureMeasure(Double.doubleToLongBits(interpolVal));
}
}
}
}
public void run() {
int bytesread = 0; // Number of bytes read from the input file.
int byteswritten = 0; // Number of bytes written to the stream.
byte databyte = 0; // The byte of data read from the file
int id;
long
measurement; // This is the word used to store all measurements - conversions are
// illustrated.
Frame mFrame = new Frame();
List<Frame> frameQueue = new LinkedList<Frame>();
// Next we write a message to the terminal to let the world know we are alive...
System.out.print("\n" + this.getName() + "::WildPointsFilter Reading ");
while (true) {
/**
* *********************************************************** Here we read a byte and write a
* byte ***********************************************************
*/
try {
id = this.ReadIdFromFilterInputPort((byte) 0);
bytesread += ID_LENGTH;
measurement = this.ReadMeasureFromFilterInputPort((byte) 0);
bytesread += MEASUREMENT_LENGTH;
if (id == TIMESTAMP_ID) {
// TimeStamp.setTimeInMillis(measurement);
// buffer time meassure
mFrame.setTimeStamp(measurement);
} // if
if (id == VELOCITY_ID) {
mFrame.setVelocity(measurement);
} // if
if (id == ALTITUDE_MEASURE_ID) {
mFrame.setAltitudeMeasure(measurement);
} // if
if (id == PRESSURE_MEASURE_ID) {
mFrame.setPressureMeasure(measurement);
} // if
if (id == TEMPERATURE_MEASURE_ID) {
mFrame.setTemperatureMeasure(measurement);
} // if
if (id == ATTITUDE_MEASURE_ID) {
mFrame.setAttitude(measurement);
// Attitude has highest ID.
// After reading attitude, we have all other needed measures
// add it last
frameQueue.add(new Frame(mFrame));
ExtrapolateFramesInList(frameQueue, false);
// get first
Frame lastEnqueued = frameQueue.get(0);
// If the frame is with valid pressure measure
// or it is extrapolated - write it down
if ((lastEnqueued.getHasInvalidPressureMeasure() == false)
|| (lastEnqueued.getHasInvalidPressureMeasure() == true
&& lastEnqueued.getExtrapolatedPressureMeasure() > 0)) {
// Writedown measure with valid pressure
lastEnqueued = frameQueue.remove(0);
WriteFrameToOutputPorts(lastEnqueued);
}
} // if
} // try
catch (EndOfStreamException e) {
// FLUSH QUEUE BEFORE CLOSING THE PORTS
while (!frameQueue.isEmpty()) {
ExtrapolateFramesInList(frameQueue, true);
Frame lastEnqueued = frameQueue.remove(0);
WriteFrameToOutputPorts(lastEnqueued);
}
ClosePorts();
System.out.print(
"\n"
+ this.getName()
+ "::Middle Exiting; bytes read: "
+ bytesread
+ " bytes written: "
+ byteswritten);
break;
} // catch
} // while
} // run
