diff --git a/CMakeLists.txt b/CMakeLists.txt index c36781c..0e5dbeb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ FetchContent_MakeAvailable(Boost) FetchContent_Declare( isobus GIT_REPOSITORY https://github.com/Open-Agriculture/AgIsoStack-plus-plus.git - GIT_TAG 495eba6653010449d6202165240da7623243f416 + GIT_TAG 935a3bc16881922c73af5aeddb7f25105f67c1c3 DOWNLOAD_EXTRACT_TIMESTAMP TRUE) FetchContent_MakeAvailable(isobus) diff --git a/include/task_controller.hpp b/include/task_controller.hpp index 9546457..eb70bf2 100644 --- a/include/task_controller.hpp +++ b/include/task_controller.hpp @@ -50,7 +50,8 @@ class ClientState bool are_measurement_commands_sent() const; 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); + const std::vector &get_element_numbers_for_ddi(isobus::DataDescriptionIndex ddi) const; + void add_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 @@ -60,7 +61,7 @@ class ClientState private: isobus::DeviceDescriptorObjectPool pool; ///< The device descriptor object pool (DDOP) for the TC bool areMeasurementCommandsSent = false; ///< Whether or not the measurement commands have been sent - std::map ddiToElementNumber; ///< Mapping of DDI to element number // TODO: better way to do this? + std::map> ddiToElementNumbers; ///< Mapping of DDI to element numbers (supports multiple booms) std::uint8_t numberOfSections; std::vector sectionSetpointStates; // 2 bits per section (0 = off, 1 = on, 2 = error, 3 = not installed) diff --git a/src/app.cpp b/src/app.cpp index e11ba02..479a5e5 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -136,6 +136,13 @@ bool Application::initialize() auto packetHandler = [this, tcCF](std::uint8_t src, std::uint8_t pgn, std::span data) { if (src == 0x7F && pgn == 0xE5) // 229 - 64 sections PGN { + if (data.size() < 8) + { + std::cout << "[Warning] Received PGN 0xE5 packet with insufficient length (" << data.size() + << "), expected at least 8 bytes. Ignoring packet." << std::endl; + return; + } + std::vector sectionStates; for (std::uint8_t j = 0; j < 8; j++) { diff --git a/src/logging.cpp b/src/logging.cpp index 2a12838..d219fd6 100644 --- a/src/logging.cpp +++ b/src/logging.cpp @@ -4,8 +4,10 @@ #include #include +#include #include #include +#include #include class TeeStreambuf : public std::streambuf @@ -48,6 +50,20 @@ class TeeStreambuf : public std::streambuf static std::unique_ptr teeStream; +static std::string get_timestamp() +{ + std::time_t now = std::time(nullptr); + std::tm localTime; + localtime_s(&localTime, &now); + + std::ostringstream oss; + oss << std::setfill('0') + << std::setw(2) << localTime.tm_hour << ":" + << std::setw(2) << localTime.tm_min << ":" + << std::setw(2) << localTime.tm_sec; + return oss.str(); +} + static void setup_file_logging() { // Generate timestamped filename @@ -74,6 +90,7 @@ class CustomLogger : public isobus::CANStackLogger void sink_CAN_stack_log(CANStackLogger::LoggingLevel level, const std::string &text) override { + std::cout << "[" << get_timestamp() << "]"; switch (level) { case LoggingLevel::Debug: diff --git a/src/task_controller.cpp b/src/task_controller.cpp index 3d7e61f..1e2bab2 100644 --- a/src/task_controller.cpp +++ b/src/task_controller.cpp @@ -12,6 +12,7 @@ #include "isobus/isobus/isobus_device_descriptor_object_pool_helpers.hpp" #include "isobus/isobus/isobus_task_controller_server.hpp" +#include #include #include #include @@ -145,23 +146,41 @@ void ClientState::mark_measurement_commands_sent() std::uint16_t ClientState::get_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const { - auto it = ddiToElementNumber.find(ddi); - if (it != ddiToElementNumber.end()) + auto it = ddiToElementNumbers.find(ddi); + if (it != ddiToElementNumbers.end() && !it->second.empty()) { - return it->second; + return it->second.front(); // Return first element for backwards compatibility } std::cout << "Cached element number not found for DDI " << static_cast(ddi) << std::endl; return 0; } -void ClientState::set_element_number_for_ddi(isobus::DataDescriptionIndex ddi, std::uint16_t elementNumber) +static const std::vector emptyElementNumbers; + +const std::vector &ClientState::get_element_numbers_for_ddi(isobus::DataDescriptionIndex ddi) const +{ + auto it = ddiToElementNumbers.find(ddi); + if (it != ddiToElementNumbers.end()) + { + return it->second; + } + return emptyElementNumbers; +} + +void ClientState::add_element_number_for_ddi(isobus::DataDescriptionIndex ddi, std::uint16_t elementNumber) { - ddiToElementNumber[ddi] = elementNumber; + auto &elements = ddiToElementNumbers[ddi]; + // Avoid duplicates + if (std::find(elements.begin(), elements.end(), elementNumber) == elements.end()) + { + elements.push_back(elementNumber); + } } bool ClientState::has_element_number_for_ddi(isobus::DataDescriptionIndex ddi) const { - return ddiToElementNumber.find(ddi) != ddiToElementNumber.end(); + auto it = ddiToElementNumbers.find(ddi); + return it != ddiToElementNumbers.end() && !it->second.empty(); } bool ClientState::is_element_or_parent_off(std::uint16_t elementNumber) const @@ -496,7 +515,7 @@ void MyTCServer::request_measurement_commands() if (elementObjectChild == processDataObject->get_object_id()) { // 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()); + client.second.add_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; @@ -543,7 +562,7 @@ void MyTCServer::request_measurement_commands() if (elementObjectChild == processDataObject->get_object_id()) { // 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()); + client.second.add_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)) @@ -595,7 +614,17 @@ void MyTCServer::update_section_states(std::vector §ionStates) if (i < sectionStates.size()) { - if (sectionStates[i] != (state.get_section_setpoint_state(i) == SectionState::ON)) + // Only control sections that are installed (actual state != NOT_INSTALLED) + if (state.get_section_actual_state(i) == SectionState::NOT_INSTALLED) + { + // Ensure setpoint also reflects NOT_INSTALLED + if (state.get_section_setpoint_state(i) != SectionState::NOT_INSTALLED) + { + state.set_section_setpoint_state(i, SectionState::NOT_INSTALLED); + requiresUpdate = true; + } + } + else if (sectionStates[i] != (state.get_section_setpoint_state(i) == SectionState::ON)) { state.set_section_setpoint_state(i, sectionStates[i] ? SectionState::ON : SectionState::OFF); requiresUpdate = true; @@ -635,15 +664,23 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr(isobus::DataDescriptionIndex::SetpointCondensedWorkState1_16) + ddiOffset; // 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); + // Send to ALL elements that have this DDI (supports multiple booms) + for (std::uint16_t elementNumber : clients[client].get_element_numbers_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) && 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); + // Send SetpointWorkState to all elements that have it + for (std::uint16_t elementNumber : clients[client].get_element_numbers_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState)) + { + send_set_value(client, static_cast(isobus::DataDescriptionIndex::SetpointWorkState), elementNumber, setpointWorkState ? 1 : 0); + } clients[client].set_setpoint_work_state(setpointWorkState); } else if (!clients[client].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState)) @@ -655,7 +692,11 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr(ddiTargetLegacy)), value); + // Send to ALL elements that have this legacy DDI + for (std::uint16_t elementNumber : clients[client].get_element_numbers_for_ddi(static_cast(ddiTargetLegacy))) + { + send_set_value(client, ddiTargetLegacy, elementNumber, value); + } } else { @@ -671,7 +712,11 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr client, bool enabled) { - send_set_value(client, static_cast(isobus::DataDescriptionIndex::SectionControlState), clients[client].get_element_number_for_ddi(isobus::DataDescriptionIndex::SectionControlState), enabled ? 1 : 0); + // Send SectionControlState to all elements that have it + for (std::uint16_t elementNumber : clients[client].get_element_numbers_for_ddi(isobus::DataDescriptionIndex::SectionControlState)) + { + send_set_value(client, static_cast(isobus::DataDescriptionIndex::SectionControlState), elementNumber, enabled ? 1 : 0); + } } bool MyTCServer::is_ddi_settable(std::shared_ptr client, std::uint16_t ddi)