|
58 | 58 | import pt.lsts.imc.def.ProtocolDefinition; |
59 | 59 | import pt.lsts.imc.def.ValueDescriptor; |
60 | 60 | import pt.lsts.imc.gz.MultiMemberGZIPInputStream; |
| 61 | +import pt.lsts.imc.lsf.BigByteBuffer; |
61 | 62 | import pt.lsts.neptus.messages.IMessageProtocol; |
62 | 63 |
|
63 | 64 | /** |
@@ -443,6 +444,62 @@ else if (sync == syncWord) |
443 | 444 | } |
444 | 445 | } |
445 | 446 |
|
| 447 | + public IMCMessage nextMessage(BigByteBuffer buff) throws Exception { |
| 448 | + IMCMessage msg; |
| 449 | + do { |
| 450 | + msg = nextMessageWorkerNonRecursive(buff); |
| 451 | + } while (msg == null); |
| 452 | + return msg; |
| 453 | + } |
| 454 | + |
| 455 | + public IMCMessage nextMessageWorkerNonRecursive(BigByteBuffer buff) throws Exception { |
| 456 | + Header header = createHeader(); |
| 457 | + long sync = buff.getShort() & 0xFFFF; |
| 458 | + if (sync == swappedWord) |
| 459 | + buff.order(ByteOrder.LITTLE_ENDIAN); |
| 460 | + else if (sync == syncWord) |
| 461 | + header.setValue("sync", syncWord); |
| 462 | + else { |
| 463 | + System.err.printf( |
| 464 | + "Found a message with invalid sync (%X) was skipped\n", |
| 465 | + sync); |
| 466 | + byte[] tmp = new byte[header.getInteger("size") + 2]; |
| 467 | + buff.get(tmp); |
| 468 | + return null; //nextMessage(buff); |
| 469 | + } |
| 470 | + |
| 471 | + byte[] tmp = new byte[header.getMessageType().getComputedLength() - 2]; |
| 472 | + buff.get(tmp); |
| 473 | + ByteBuffer byteBuffer = ByteBuffer.wrap(tmp); |
| 474 | + byteBuffer.order(buff.order()); |
| 475 | + |
| 476 | + deserializeAllFieldsBut(header, byteBuffer, "sync"); |
| 477 | + |
| 478 | + // get the remainer of the message |
| 479 | + tmp = new byte[header.getInteger("size") + 2]; |
| 480 | + buff.get(tmp); |
| 481 | + byteBuffer = ByteBuffer.wrap(tmp); |
| 482 | + byteBuffer.order(buff.order()); |
| 483 | + |
| 484 | + IMCMessageType type = getType(getMessageName(header.get_mgid())); |
| 485 | + if (type != null) { |
| 486 | + IMCMessage message = MessageFactory |
| 487 | + .getInstance() |
| 488 | + .createTypedMessage(getMessageName(header.get_mgid()), this); |
| 489 | + message.setHeader(header); |
| 490 | + message.setType(type); |
| 491 | + deserializeFields(message, byteBuffer); |
| 492 | + deserialize(IMCFieldType.TYPE_UINT16, byteBuffer); // footer |
| 493 | + return message; |
| 494 | + } else { |
| 495 | + System.err.println("Unknown message type was skipped: " |
| 496 | + + header.getInteger("mgid")); |
| 497 | + // byte[] tmp = new byte[header.getInteger("size") + 2]; |
| 498 | + // buff.get(tmp); |
| 499 | + return null; //nextMessage(buff); |
| 500 | + } |
| 501 | + } |
| 502 | + |
446 | 503 | /** |
447 | 504 | * Read a message header from the given IMCInputStream |
448 | 505 | * |
|
0 commit comments