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