[WS]: Fixed macro command packets being limited to 8 bytes

Fixed an issue where change child position command packets inside macros would be truncated due to us assuming command packets would be 8 bytes.
Open-Agriculture · Apr 25, 2024 · 1b51754 · 1b51754
1 parent 5a10b96
commit 1b51754
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 14 deletions.
diff --git 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<std::uint8_t, 8> commandPacket = { 0 };
+					std::vector<std::uint8_t> 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
@@ -2882,40 +2882,54 @@ namespace isobus
 						tempObject->set_id(decodedID);
 
 						auto numberBytesToFollow = static_cast<std::uint16_t>(static_cast<std::uint16_t>(iopData[3]) | (static_cast<std::uint16_t>(iopData[4]) << 8));
+						std::uint16_t numberBytesProcessed = 0;
 						iopLength -= 5;
 						iopData += 5;
 
 						if (iopLength >= numberBytesToFollow)
 						{
 							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;
 								}
 							}