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 0c10ed3..e11ba02 100644 --- a/src/app.cpp +++ b/src/app.cpp @@ -134,19 +134,16 @@ bool Application::initialize() static std::uint32_t lastXteTransmit = 0; auto packetHandler = [this, tcCF](std::uint8_t src, std::uint8_t pgn, std::span data) { - if (src == 0x7F && pgn == 0xFE) // 254 - Steer Data + if (src == 0x7F && pgn == 0xE5) // 229 - 64 sections PGN { - // TODO: hack to get desired section states. probably want to make a new pgn later when we need more than 16 sections std::vector sectionStates; - for (std::uint8_t i = 0; i < 8; i++) + for (std::uint8_t j = 0; j < 8; j++) { - sectionStates.push_back(data[6] & (1 << i)); - } - for (std::uint8_t i = 0; i < 8; i++) - { - sectionStates.push_back(data[7] & (1 << i)); + for (std::uint8_t i = 0; i < 8; i++) + { + sectionStates.push_back(data[j] & (1 << i)); + } } - tcServer->update_section_states(sectionStates); } else if (src == 0x7F && pgn == 0xF1) // 241 - Section Control 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 72a0567..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 @@ -228,8 +247,8 @@ bool ClientState::try_get_element_work_state(std::uint16_t elementNumber, bool & MyTCServer::MyTCServer(std::shared_ptr internalControlFunction) : TaskControllerServer(internalControlFunction, 1, // AOG limits to 1 boom - 16, // AOG limits to 16 sections of unique width - 16, // 16 channels for position based control + 64, // AOG limits to 16 sections of unique width but can be 64 by using zones + 64, // 64 channels for position based control isobus::TaskControllerOptions() .with_implement_section_control(), // We support section control TaskControllerVersion::SecondEditionDraft) @@ -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)