diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 4e7685b..9546457 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -9,6 +9,7 @@ #pragma once +#include "isobus/isobus/isobus_data_dictionary.hpp" #include "isobus/isobus/isobus_device_descriptor_object_pool.hpp" #include "isobus/isobus/isobus_standard_data_description_indices.hpp" #include "isobus/isobus/isobus_task_controller_server.hpp" @@ -36,6 +37,8 @@ class ClientState std::uint8_t get_number_of_sections() const; std::uint8_t get_section_setpoint_state(std::uint8_t section) const; std::uint8_t get_section_actual_state(std::uint8_t section) const; + std::uint16_t get_element_number_for_section(std::uint8_t section) const; + void set_element_number_for_section(std::uint8_t section, std::uint16_t elementNumber); bool is_any_section_setpoint_on() const; bool get_setpoint_work_state() const; void set_setpoint_work_state(bool state); @@ -48,6 +51,11 @@ class ClientState void mark_measurement_commands_sent(); std::uint16_t get_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const; void set_element_number_for_ddi(isobus::DataDescriptionIndex ddi, std::uint16_t elementNumber); + bool has_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const; + bool is_element_or_parent_off(std::uint16_t elementNumber) const; ///< Recursively checks if element or any parent is off + // Element work state management these act like master / override for actual sections + void set_element_work_state(std::uint16_t elementNumber, bool isWorking); + bool try_get_element_work_state(std::uint16_t elementNumber, bool &isWorking) const; private: isobus::DeviceDescriptorObjectPool pool; ///< The device descriptor object pool (DDOP) for the TC @@ -57,8 +65,10 @@ class ClientState std::uint8_t numberOfSections; std::vector sectionSetpointStates; // 2 bits per section (0 = off, 1 = on, 2 = error, 3 = not installed) std::vector sectionActualStates; // 2 bits per section (0 = off, 1 = on, 2 = error, 3 = not installed) - bool setpointWorkState = false; ///< The overall work state desired + std::vector sectionToElementNumber; // Maps section index to element number for hierarchy checking + bool setpointWorkState = false; ///< The overall work state desired (DDI 289) bool actualWorkState = false; ///< The overall work state actual + std::map elementWorkStates; ///< Work state per element (element number -> is working) bool isSectionControlEnabled = false; ///< Stores auto vs manual mode setting }; @@ -91,6 +101,7 @@ class MyTCServer : public isobus::TaskControllerServer private: void send_section_setpoint_states(std::shared_ptr client, std::uint8_t ddiOffset); void send_section_control_state(std::shared_ptr client, bool enabled); + bool is_ddi_settable(std::shared_ptr client, std::uint16_t ddi); std::map, ClientState> clients; std::map, std::queue>> uploadedPools; diff --git a/src/app.cpp b/src/app.cpp index ef3b463..06755ce 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -41,6 +41,8 @@ bool Application::initialize() return false; } + isobus::CANNetworkManager::CANNetwork.get_configuration().set_number_of_packets_per_cts_message(255); + isobus::NAME ourNAME(0); //! Make sure you change these for your device!!!! diff --git a/src/main.cpp b/src/main.cpp index 12274a7..6ec6b6c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -116,8 +116,8 @@ class ArgumentProcessor std::cout << "Options:\n"; std::cout << " --help\t\tShow this help message\n"; std::cout << " --version\t\tShow the version of the application\n"; - std::cout << " --adapter=\tSelect the CAN driver\n"; - std::cout << " --channel=\tSelect the CAN channel\n"; + std::cout << " --can_adapter=\tSelect the CAN driver\n"; + std::cout << " --can_channel=\tSelect the CAN channel\n"; std::cout << " --log_level=\tSet the log level (debug, info, warning, error, critical)\n"; std::cout << " --log2file\t\tLog to file\n"; exit(0); diff --git a/src/task_controller.cpp b/src/task_controller.cpp index d776345..4ee92bb 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -21,6 +21,7 @@ void ClientState::set_number_of_sections(std::uint8_t number) numberOfSections = number; sectionSetpointStates.resize(number); sectionActualStates.resize(number); + sectionToElementNumber.resize(number, 0); // Initialize all sections mapped to element 0 by default } void ClientState::set_section_setpoint_state(std::uint8_t section, std::uint8_t state) @@ -39,6 +40,23 @@ void ClientState::set_section_actual_state(std::uint8_t section, std::uint8_t st } } +std::uint16_t ClientState::get_element_number_for_section(std::uint8_t section) const +{ + if (section < numberOfSections && section < sectionToElementNumber.size()) + { + return sectionToElementNumber[section]; + } + return 0; +} + +void ClientState::set_element_number_for_section(std::uint8_t section, std::uint16_t elementNumber) +{ + if (section < numberOfSections && section < sectionToElementNumber.size()) + { + sectionToElementNumber[section] = elementNumber; + } +} + std::uint8_t ClientState::get_number_of_sections() const { return numberOfSections; @@ -57,6 +75,12 @@ std::uint8_t ClientState::get_section_actual_state(std::uint8_t section) const { if (section < numberOfSections) { + // Check if the element or any parent is off + std::uint16_t elementNumber = get_element_number_for_section(section); + if (is_element_or_parent_off(elementNumber)) + { + return SectionState::OFF; + } return sectionActualStates[section]; } return SectionState::NOT_INSTALLED; @@ -135,6 +159,72 @@ void ClientState::set_element_number_for_ddi(isobus::DataDescriptionIndex ddi, s ddiToElementNumber[ddi] = elementNumber; } +bool ClientState::has_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const +{ + return ddiToElementNumber.find(ddi) != ddiToElementNumber.end(); +} + +bool ClientState::is_element_or_parent_off(std::uint16_t elementNumber) const +{ + // Check if this element is off + bool elementWorkState; + if (try_get_element_work_state(elementNumber, elementWorkState) && !elementWorkState) + { + return true; // Element is off + } + + // Find the parent element(s) by searching through the pool + // Use const_cast to access non-const methods on the pool from a const context + auto &nonConstPool = const_cast(pool); + for (std::uint32_t i = 0; i < nonConstPool.size(); i++) + { + auto object = nonConstPool.get_object_by_index(i); + if (object->get_object_type() == isobus::task_controller_object::ObjectTypes::DeviceElement) + { + auto elementObject = std::dynamic_pointer_cast(object); + // Check if this element has elementNumber as a child + for (std::uint16_t childId : elementObject->get_child_object_ids()) + { + // Find the child object + for (std::uint32_t j = 0; j < nonConstPool.size(); j++) + { + auto childObject = nonConstPool.get_object_by_index(j); + if (childObject && childObject->get_object_id() == childId) + { + if (childObject->get_object_type() == isobus::task_controller_object::ObjectTypes::DeviceElement) + { + auto childElementObject = std::dynamic_pointer_cast(childObject); + if (childElementObject && childElementObject->get_element_number() == elementNumber) + { + // Found the parent, recursively check if parent or its parents are off + return is_element_or_parent_off(elementObject->get_element_number()); + } + } + } + } + } + } + } + + return false; // No parent found or no parents are off +} + +void ClientState::set_element_work_state(std::uint16_t elementNumber, bool isWorking) +{ + elementWorkStates[elementNumber] = isWorking; +} + +bool ClientState::try_get_element_work_state(std::uint16_t elementNumber, bool &isWorking) const +{ + auto it = elementWorkStates.find(elementNumber); + if (it != elementWorkStates.end()) + { + isWorking = it->second; + return true; + } + return false; +} + MyTCServer::MyTCServer(std::shared_ptr internalControlFunction) : TaskControllerServer(internalControlFunction, 1, // AOG limits to 1 boom @@ -329,7 +419,9 @@ bool MyTCServer::on_value_command(std::shared_ptr partn for (std::uint_fast8_t i = 0; i < NUMBER_SECTIONS_PER_CONDENSED_MESSAGE; i++) { - clients[partner].set_section_actual_state(i + sectionIndexOffset, (processDataValue >> (2 * i)) & 0x03); + std::uint8_t sectionState = ((processDataValue >> (2 * i)) & 0x03); + clients[partner].set_section_actual_state(i + sectionIndexOffset, sectionState); + clients[partner].set_element_number_for_section(i + sectionIndexOffset, elementNumber); } } break; @@ -342,7 +434,8 @@ bool MyTCServer::on_value_command(std::shared_ptr partn case static_cast(isobus::DataDescriptionIndex::ActualWorkState): { - clients[partner].set_setpoint_work_state(processDataValue == 1); + // Store the work state per element rather than globally + clients[partner].set_element_work_state(elementNumber, processDataValue == 1); } } @@ -379,7 +472,9 @@ void MyTCServer::request_measurement_commands() auto processDataObject = std::dynamic_pointer_cast(object); if (processDataObject->get_ddi() == static_cast(isobus::DataDescriptionIndex::ActualWorkState) || (processDataObject->get_ddi() >= static_cast(isobus::DataDescriptionIndex::ActualCondensedWorkState1_16) && - processDataObject->get_ddi() <= static_cast(isobus::DataDescriptionIndex::ActualCondensedWorkState241_256))) + processDataObject->get_ddi() <= static_cast(isobus::DataDescriptionIndex::ActualCondensedWorkState241_256)) || + (processDataObject->get_ddi() >= static_cast(isobus::DataDescriptionIndex::CondensedSectionOverrideState1_16) && + processDataObject->get_ddi() <= static_cast(isobus::DataDescriptionIndex::CondensedSectionOverrideState241_256))) { // Loop over all objects to find the elements that are the parents of the actual condensed work state objects for (std::uint32_t j = 0; j < client.second.get_pool().size(); j++) @@ -394,10 +489,15 @@ void MyTCServer::request_measurement_commands() { // TODO: This is a bit of a hack, but it works for now client.second.set_element_number_for_ddi(static_cast(processDataObject->get_ddi()), elementObject->get_element_number()); + const auto &entryB = isobus::DataDictionary::get_entry(processDataObject->get_ddi()); + std::cout << "Mapped DDI " << processDataObject->get_ddi() << " (" << entryB.to_string() << ") to element " + << elementObject->get_element_number() << std::endl; if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::OnChange)) { send_change_threshold_measurement_command(client.first, processDataObject->get_ddi(), elementObject->get_element_number(), 1); + std::cout << "Subscribed (OnChange) to DDI " << processDataObject->get_ddi() << " (" << entryB.to_string() << ") for element " + << elementObject->get_element_number() << std::endl; } if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::TimeInterval)) { @@ -436,10 +536,18 @@ void MyTCServer::request_measurement_commands() { // TODO: This is a bit of a hack, but it works for now client.second.set_element_number_for_ddi(static_cast(processDataObject->get_ddi()), elementObject->get_element_number()); + const auto &entryB = isobus::DataDictionary::get_entry(processDataObject->get_ddi()); if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::OnChange)) { send_change_threshold_measurement_command(client.first, processDataObject->get_ddi(), elementObject->get_element_number(), 1); + std::cout << "Subscribed (OnChange) to DDI " << processDataObject->get_ddi() << " (" << entryB.to_string() << ") for element " + << elementObject->get_element_number() << std::endl; + } + else + { + std::cout << "Mapped (no OnChange) DDI " << processDataObject->get_ddi() << " (" << entryB.to_string() << ") to element " + << elementObject->get_element_number() << std::endl; } } } @@ -488,7 +596,7 @@ void MyTCServer::update_section_states(std::vector §ionStates) } if (requiresUpdate) { - std::uint8_t ddiOffset = state.get_number_of_sections() / NUMBER_SECTIONS_PER_CONDENSED_MESSAGE; + std::uint8_t ddiOffset = (state.get_number_of_sections() - 1) / NUMBER_SECTIONS_PER_CONDENSED_MESSAGE; send_section_setpoint_states(client.first, ddiOffset); } } @@ -515,15 +623,41 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr(isobus::DataDescriptionIndex::SetpointCondensedWorkState1_16) + ddiOffset; - std::uint16_t elementNumber = clients[client].get_element_number_for_ddi(static_cast(ddiTarget)); - send_set_value(client, ddiTarget, elementNumber, value); + // Legacy ECU? (DDI 161 ActualCondensedWorkState1_16 exists and Settable) + std::uint16_t ddiTargetLegacy = static_cast(isobus::DataDescriptionIndex::ActualCondensedWorkState1_16) + ddiOffset; + if (clients[client].has_element_number_for_ddi(static_cast(ddiTarget))) + { + std::uint16_t elementNumber = clients[client].get_element_number_for_ddi(static_cast(ddiTarget)); + send_set_value(client, ddiTarget, elementNumber, value); - bool setpointWorkState = clients[client].is_any_section_setpoint_on(); - if ((clients[client].get_setpoint_work_state() != setpointWorkState)) + bool setpointWorkState = clients[client].is_any_section_setpoint_on(); + if ((clients[client].get_setpoint_work_state() != setpointWorkState) && clients[client].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState)) + { + send_set_value(client, static_cast(isobus::DataDescriptionIndex::SetpointWorkState), clients[client].get_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState), setpointWorkState ? 1 : 0); + clients[client].set_setpoint_work_state(setpointWorkState); + } + else if (!clients[client].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState)) + { + std::cout << "[TC Server] DDI 289 (SetpointWorkState) not available!" << std::endl; + } + } + else if (clients[client].has_element_number_for_ddi(static_cast(ddiTargetLegacy))) { - send_set_value(client, static_cast(isobus::DataDescriptionIndex::SetpointWorkState), clients[client].get_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState), setpointWorkState ? 1 : 0); - clients[client].set_setpoint_work_state(setpointWorkState); + if (is_ddi_settable(client, ddiTargetLegacy)) + { + send_set_value(client, ddiTargetLegacy, clients[client].get_element_number_for_ddi(static_cast(ddiTargetLegacy)), value); + } + else + { + std::cout << "[TC Server] Legacy DDI " << ddiTargetLegacy << " (ActualCondensedWorkState) is not settable!" << std::endl; + } + return; + } + else + { + std::cout << "[TC Server] Neither condensed nor controllable-actual work state supported Missing DDI 290 and 141!" << std::endl; } } @@ -531,3 +665,20 @@ void MyTCServer::send_section_control_state(std::shared_ptr(isobus::DataDescriptionIndex::SectionControlState), clients[client].get_element_number_for_ddi(isobus::DataDescriptionIndex::SectionControlState), enabled ? 1 : 0); } + +bool MyTCServer::is_ddi_settable(std::shared_ptr client, std::uint16_t ddi) +{ + for (std::uint32_t i = 0; i < clients[client].get_pool().size(); i++) + { + auto object = clients[client].get_pool().get_object_by_index(i); + if (object->get_object_type() == isobus::task_controller_object::ObjectTypes::DeviceProcessData) + { + auto processDataObject = std::dynamic_pointer_cast(object); + if (processDataObject->get_ddi() == ddi) + { + return processDataObject->has_property(isobus::task_controller_object::DeviceProcessDataObject::PropertiesBit::Settable); + } + } + } + return false; +}