From d9f64eeaa487babc8cd40c18850e9ae6179da04a Mon Sep 17 00:00:00 2001 From: Daan Steenbergen Date: Tue, 12 Dec 2023 20:52:26 +0100 Subject: [PATCH] fix(vt): wait on response before processing next command --- .../isobus/isobus_virtual_terminal_client.hpp | 13 +-- isobus/src/isobus_virtual_terminal_client.cpp | 95 +++++++++++++++---- test/vt_client_tests.cpp | 90 +++++++++++------- 3 files changed, 138 insertions(+), 60 deletions(-) diff --git a/isobus/include/isobus/isobus/isobus_virtual_terminal_client.hpp b/isobus/include/isobus/isobus/isobus_virtual_terminal_client.hpp index 4c0e6c72..0b027284 100644 --- a/isobus/include/isobus/isobus/isobus_virtual_terminal_client.hpp +++ b/isobus/include/isobus/isobus/isobus_virtual_terminal_client.hpp @@ -23,6 +23,7 @@ #include #if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO +#include #include #endif @@ -533,10 +534,6 @@ namespace isobus void update_auxiliary_input(const std::uint16_t auxiliaryInputID, const std::uint16_t value1, const std::uint16_t value2, const bool controlLocked = false); // Command Messages - /// @brief Set whether the client should queue commands when sending them to the VT server failed first try. (Default: true). - /// This can happen if a transport protocol is busy with sending another message. - /// @param[in] shouldQueueCommands Whether the client should queue commands when sending them to the VT server failed first try - void set_should_queue_commands(const bool shouldQueueCommands); /// @brief Sends a hide/show object command /// @details This command is used to hide or show a Container object. @@ -1573,7 +1570,7 @@ namespace isobus /// @brief Sends a command to the VT server /// @param[in] data The data to send, including the function-code /// @returns true if the message was sent successfully - bool send_command(const std::vector &data) const; + bool send_command(const std::vector &data); /// @brief Tries to send a command to the VT server, and queues it if it fails /// @param[in] data The data to send, including the function-code @@ -1650,7 +1647,11 @@ namespace isobus // Command queue std::vector> commandQueue; ///< A queue of commands to send to the VT server - bool commandQueueEnabled = true; ///< Determines if the command queue is enabled + bool commandAwaitingResponse = false; ///< Determines if we are currently waiting for a response to a command + std::uint32_t lastCommandTimestamp_ms = 0; ///< The timestamp of the last command sent +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::mutex commandQueueMutex; ///< A mutex to protect the command queue +#endif // Activation event callbacks EventDispatcher softKeyEventDispatcher; ///< A list of all soft key event callbacks diff --git a/isobus/src/isobus_virtual_terminal_client.cpp b/isobus/src/isobus_virtual_terminal_client.cpp index 7902d2eb..5f1fa5c3 100644 --- a/isobus/src/isobus_virtual_terminal_client.cpp +++ b/isobus/src/isobus_virtual_terminal_client.cpp @@ -250,11 +250,6 @@ namespace isobus } } - void VirtualTerminalClient::set_should_queue_commands(const bool shouldQueueCommands) - { - commandQueueEnabled = shouldQueueCommands; - } - isobus::VirtualTerminalClient::AssignedAuxiliaryFunction::AssignedAuxiliaryFunction(const std::uint16_t functionObjectID, const std::uint16_t inputObjectID, const AuxiliaryTypeTwoFunctionType functionType) : functionObjectID(functionObjectID), inputObjectID(inputObjectID), functionType(functionType) { @@ -1765,7 +1760,6 @@ namespace isobus { static constexpr std::uint8_t SUPPORTED_VT_VERSION = 0x06; - std::uint8_t versionByte; std::uint8_t bitmask = (initializing ? 0x01 : 0x00); const std::array buffer = { static_cast(Function::WorkingSetMaintenanceMessage), @@ -3033,6 +3027,50 @@ namespace isobus } } break; + + case static_cast(Function::HideShowObjectCommand): + case static_cast(Function::EnableDisableObjectCommand): + case static_cast(Function::SelectInputObjectCommand): + case static_cast(Function::ESCCommand): + case static_cast(Function::ControlAudioSignalCommand): + case static_cast(Function::SetAudioVolumeCommand): + case static_cast(Function::ChangeChildLocationCommand): + case static_cast(Function::ChangeChildPositionCommand): + case static_cast(Function::ChangeSizeCommand): + case static_cast(Function::ChangeBackgroundColourCommand): + case static_cast(Function::ChangeNumericValueCommand): + case static_cast(Function::ChangeStringValueCommand): + case static_cast(Function::ChangeEndPointCommand): + case static_cast(Function::ChangeFontAttributesCommand): + case static_cast(Function::ChangeLineAttributesCommand): + case static_cast(Function::ChangeFillAttributesCommand): + case static_cast(Function::ChangeActiveMaskCommand): + case static_cast(Function::ChangeSoftKeyMaskCommand): + case static_cast(Function::ChangeAttributeCommand): + case static_cast(Function::ChangePriorityCommand): + case static_cast(Function::ChangeListItemCommand): + case static_cast(Function::DeleteObjectPoolCommand): + case static_cast(Function::LockUnlockMaskCommand): + case static_cast(Function::ExecuteMacroCommand): + case static_cast(Function::ChangeObjectLabelCommand): + case static_cast(Function::ChangePolygonPointCommand): + case static_cast(Function::ChangePolygonScaleCommand): + case static_cast(Function::GraphicsContextCommand): + case static_cast(Function::GetAttributeValueMessage): + case static_cast(Function::SelectColourMapCommand): + case static_cast(Function::ExecuteExtendedMacroCommand): + case static_cast(Function::SelectActiveWorkingSet): + { + // By checking if it's a response with our control functions, we verify that it's a response to a request we sent. + // This because we only support Working Set Masters at the moment. + if ((parentVT->myControlFunction == message.get_destination_control_function()) && + (parentVT->partnerControlFunction == message.get_source_control_function())) + { + parentVT->commandAwaitingResponse = false; + parentVT->process_command_queue(); + } + } + break; } } break; @@ -4288,20 +4326,40 @@ namespace isobus return retVal; } - bool VirtualTerminalClient::send_command(const std::vector &data) const + bool VirtualTerminalClient::send_command(const std::vector &data) { + if (commandAwaitingResponse) + { + if (SystemTiming::time_expired_ms(lastCommandTimestamp_ms, 1500)) + { + CANStackLogger::warn("[VT]: Server response to a command timed out"); + commandAwaitingResponse = false; + } + else + { + // We're still waiting for a response to the last command, so we can't send another one yet + return false; + } + } + if (!get_is_connected()) { CANStackLogger::error("[VT]: Cannot send command, not connected"); return false; } - return CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), - data.data(), - data.size(), - myControlFunction, - partnerControlFunction, - CANIdentifier::CANPriority::Priority5); + bool success = CANNetworkManager::CANNetwork.send_can_message(static_cast(CANLibParameterGroupNumber::ECUtoVirtualTerminal), + data.data(), + data.size(), + myControlFunction, + partnerControlFunction, + CANIdentifier::CANPriority::Priority5); + if (success) + { + commandAwaitingResponse = true; + lastCommandTimestamp_ms = SystemTiming::get_timestamp_ms(); + } + return success; } bool VirtualTerminalClient::queue_command(const std::vector &data, bool replace) @@ -4311,16 +4369,14 @@ namespace isobus return true; } - if (!commandQueueEnabled) - { - return false; - } - if (replace && replace_command(data)) { return true; } +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::lock_guard lock(commandQueueMutex); +#endif commandQueue.emplace_back(data); return true; } @@ -4350,6 +4406,9 @@ namespace isobus void VirtualTerminalClient::process_command_queue() { +#if !defined CAN_STACK_DISABLE_THREADS && !defined ARDUINO + std::lock_guard lock(commandQueueMutex); +#endif for (auto it = commandQueue.begin(); it != commandQueue.end();) { if (send_command(*it)) diff --git a/test/vt_client_tests.cpp b/test/vt_client_tests.cpp index f3bd0155..e9d523b6 100644 --- a/test/vt_client_tests.cpp +++ b/test/vt_client_tests.cpp @@ -895,6 +895,7 @@ TEST(VIRTUAL_TERMINAL_TESTS, MessageConstruction) CANNetworkManager::process_receive_can_message_frame(testFrame); DerivedTestVTClient interfaceUnderTest(vtPartner, internalECU); + interfaceUnderTest.initialize(false); std::this_thread::sleep_for(std::chrono::milliseconds(50)); @@ -908,11 +909,11 @@ TEST(VIRTUAL_TERMINAL_TESTS, MessageConstruction) // Test send change active mask command while not connected queues the command ASSERT_TRUE(interfaceUnderTest.send_change_active_mask(123, 456)); + ASSERT_TRUE(serverVT.get_queue_empty()); interfaceUnderTest.test_wrapper_set_state(VirtualTerminalClient::StateMachineState::Connected); interfaceUnderTest.test_wrapper_process_command_queue(); - serverVT.read_frame(testFrame); - + ASSERT_TRUE(serverVT.read_frame(testFrame)); EXPECT_EQ(0, testFrame.channel); EXPECT_EQ(CAN_DATA_LENGTH, testFrame.dataLength); EXPECT_TRUE(testFrame.isExtendedFrame); @@ -926,10 +927,28 @@ TEST(VIRTUAL_TERMINAL_TESTS, MessageConstruction) std::uint16_t newActiveMaskObjectID = (static_cast(testFrame.data[3]) | (static_cast(testFrame.data[4]) << 8)); EXPECT_EQ(456, newActiveMaskObjectID); - // Test send_hide_show_object - interfaceUnderTest.send_hide_show_object(1234, VirtualTerminalClient::HideShowObjectCommand::HideObject); + // Test send_hide_show_object, but since we have not yet sent a response to the change active mask command, it should queue the command + ASSERT_TRUE(interfaceUnderTest.send_hide_show_object(1234, VirtualTerminalClient::HideShowObjectCommand::HideObject)); + ASSERT_TRUE(serverVT.get_queue_empty()); + interfaceUnderTest.test_wrapper_process_command_queue(); + ASSERT_FALSE(serverVT.read_frame(testFrame)); + + // Send a response to the change active mask command + testFrame.identifier = 0x14E63726; // VT->ECU + testFrame.data[0] = 173; // VT Function + testFrame.data[1] = 123 & 0xFF; + testFrame.data[2] = (123 >> 8) & 0xFF; + testFrame.data[3] = 0; // Success + testFrame.data[4] = 0xFF; // Reserved + testFrame.data[5] = 0xFF; // Reserved + testFrame.data[6] = 0xFF; // Reserved + testFrame.data[7] = 0xFF; // Reserved + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); + + interfaceUnderTest.test_wrapper_process_command_queue(); - serverVT.read_frame(testFrame); + ASSERT_TRUE(serverVT.read_frame(testFrame)); EXPECT_EQ(0, testFrame.channel); EXPECT_EQ(CAN_DATA_LENGTH, testFrame.dataLength); EXPECT_TRUE(testFrame.isExtendedFrame); @@ -944,24 +963,22 @@ TEST(VIRTUAL_TERMINAL_TESTS, MessageConstruction) EXPECT_EQ(0xFF, testFrame.data[6]); // Reserved EXPECT_EQ(0xFF, testFrame.data[7]); // Reserved - interfaceUnderTest.send_hide_show_object(1234, VirtualTerminalClient::HideShowObjectCommand::ShowObject); - serverVT.read_frame(testFrame); - EXPECT_EQ(0, testFrame.channel); - EXPECT_EQ(CAN_DATA_LENGTH, testFrame.dataLength); - EXPECT_TRUE(testFrame.isExtendedFrame); - EXPECT_EQ(0x14E72637, testFrame.identifier); - EXPECT_EQ(160, testFrame.data[0]); // VT function - - objectID = (static_cast(testFrame.data[1]) | (static_cast(testFrame.data[2]) << 8)); - EXPECT_EQ(1234, objectID); - EXPECT_EQ(1, testFrame.data[3]); // Show - EXPECT_EQ(0xFF, testFrame.data[4]); // Reserved - EXPECT_EQ(0xFF, testFrame.data[5]); // Reserved - EXPECT_EQ(0xFF, testFrame.data[6]); // Reserved - EXPECT_EQ(0xFF, testFrame.data[7]); // Reserved + // Send a response to the hide object command + testFrame.identifier = 0x14E63726; // VT->ECU + testFrame.data[0] = 160; // VT Function + testFrame.data[1] = 1234 & 0xFF; + testFrame.data[2] = (1234 >> 8) & 0xFF; + testFrame.data[3] = 0; // Hide + testFrame.data[4] = 0xFF; // Reserved + testFrame.data[5] = 0xFF; // Reserved + testFrame.data[6] = 0xFF; // Reserved + testFrame.data[7] = 0xFF; // Reserved + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); - interfaceUnderTest.send_enable_disable_object(1234, VirtualTerminalClient::EnableDisableObjectCommand::DisableObject); - serverVT.read_frame(testFrame); + ASSERT_TRUE(serverVT.get_queue_empty()); + ASSERT_TRUE(interfaceUnderTest.send_enable_disable_object(1234, VirtualTerminalClient::EnableDisableObjectCommand::DisableObject)); + ASSERT_TRUE(serverVT.read_frame(testFrame)); EXPECT_EQ(0, testFrame.channel); EXPECT_EQ(CAN_DATA_LENGTH, testFrame.dataLength); EXPECT_TRUE(testFrame.isExtendedFrame); @@ -971,21 +988,24 @@ TEST(VIRTUAL_TERMINAL_TESTS, MessageConstruction) EXPECT_EQ(1234, objectID); EXPECT_EQ(0, testFrame.data[3]); // Disable - interfaceUnderTest.send_enable_disable_object(1234, VirtualTerminalClient::EnableDisableObjectCommand::EnableObject); - serverVT.read_frame(testFrame); - EXPECT_EQ(0, testFrame.channel); - EXPECT_EQ(CAN_DATA_LENGTH, testFrame.dataLength); - EXPECT_TRUE(testFrame.isExtendedFrame); - EXPECT_EQ(0x14E72637, testFrame.identifier); - EXPECT_EQ(161, testFrame.data[0]); // VT function - objectID = (static_cast(testFrame.data[1]) | (static_cast(testFrame.data[2]) << 8)); - EXPECT_EQ(1234, objectID); - EXPECT_EQ(1, testFrame.data[3]); // Disable + // Send a response to the disable object command + testFrame.identifier = 0x14E63726; // VT->ECU + testFrame.data[0] = 161; // VT Function + testFrame.data[1] = 1234 & 0xFF; + testFrame.data[2] = (1234 >> 8) & 0xFF; + testFrame.data[3] = 0; // Disable + testFrame.data[4] = 0xFF; // Reserved + testFrame.data[5] = 0xFF; // Reserved + testFrame.data[6] = 0xFF; // Reserved + testFrame.data[7] = 0xFF; // Reserved + CANNetworkManager::process_receive_can_message_frame(testFrame); + CANNetworkManager::CANNetwork.update(); // Test draw text const std::string testString = "a"; - interfaceUnderTest.send_draw_text(123, true, 1, testString.data()); - serverVT.read_frame(testFrame); + ASSERT_TRUE(serverVT.get_queue_empty()); + ASSERT_TRUE(interfaceUnderTest.send_draw_text(123, true, 1, testString.data())); + ASSERT_TRUE(serverVT.read_frame(testFrame)); EXPECT_EQ(0, testFrame.channel); EXPECT_EQ(CAN_DATA_LENGTH, testFrame.dataLength); EXPECT_TRUE(testFrame.isExtendedFrame); @@ -997,8 +1017,6 @@ TEST(VIRTUAL_TERMINAL_TESTS, MessageConstruction) EXPECT_EQ(1, testFrame.data[5]); // Length EXPECT_EQ('a', testFrame.data[6]); - //! @todo test send command that utilizes transport protocol so queue also gets tested - serverVT.close(); CANHardwareInterface::stop();