diff --git a/isobus/src/isobus_virtual_terminal_server.cpp b/isobus/src/isobus_virtual_terminal_server.cpp index 40374529..605d6f09 100644 --- a/isobus/src/isobus_virtual_terminal_server.cpp +++ b/isobus/src/isobus_virtual_terminal_server.cpp @@ -173,7 +173,7 @@ namespace isobus for (std::uint8_t j = 0; j < macro->get_number_of_commands(); j++) { - std::array commandPacket = { 0 }; + std::vector commandPacket; if (macro->get_command_packet(j, commandPacket)) { diff --git a/isobus/src/isobus_virtual_terminal_server_managed_working_set.cpp b/isobus/src/isobus_virtual_terminal_server_managed_working_set.cpp index a7788dc1..c441fde3 100644 --- a/isobus/src/isobus_virtual_terminal_server_managed_working_set.cpp +++ b/isobus/src/isobus_virtual_terminal_server_managed_working_set.cpp @@ -2882,6 +2882,7 @@ namespace isobus tempObject->set_id(decodedID); auto numberBytesToFollow = static_cast(static_cast(iopData[3]) | (static_cast(iopData[4]) << 8)); + std::uint16_t numberBytesProcessed = 0; iopLength -= 5; iopData += 5; @@ -2889,33 +2890,46 @@ namespace isobus { retVal = true; - for (std::uint16_t i = 0; i < (numberBytesToFollow / CAN_DATA_LENGTH); i++) + while (numberBytesProcessed < numberBytesToFollow) { - retVal = tempObject->add_command_packet({ - iopData[0], - iopData[1], - iopData[2], - iopData[3], - iopData[4], - iopData[5], - iopData[6], - iopData[7], - }); - if (180 == iopData[0]) // Special case for change child position, which is 9 bytes. { + retVal = tempObject->add_command_packet({ + iopData[0], + iopData[1], + iopData[2], + iopData[3], + iopData[4], + iopData[5], + iopData[6], + iopData[7], + iopData[8], + }); iopLength -= CAN_DATA_LENGTH + 1; iopData += CAN_DATA_LENGTH + 1; + numberBytesProcessed += CAN_DATA_LENGTH + 1; } else { + retVal = tempObject->add_command_packet({ + iopData[0], + iopData[1], + iopData[2], + iopData[3], + iopData[4], + iopData[5], + iopData[6], + iopData[7], + }); iopLength -= CAN_DATA_LENGTH; iopData += CAN_DATA_LENGTH; + numberBytesProcessed += CAN_DATA_LENGTH; } if (!retVal) { - CANStackLogger::error("[WS]: Macro object %u cannot be parsed because there is not enough IOP data left", decodedID); + CANStackLogger::error("[WS]: Macro object %u cannot be parsed because a command packet could not be added.", decodedID); + break; } }