Skip to content

Keep implements in automatic mode on AOG's current section demand #106

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We鈥檒l occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
gunicsba wants to merge 1 commit into
base: develop
Choose a base branch
from
Draft

Keep implements in automatic mode on AOG's current section demand #106

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 25 additions & 1 deletion include/task_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,11 @@ class ClientState
// 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;
/// @brief Asks for every section setpoint to be sent on the next MyTCServer::update_section_setpoints()
void request_full_setpoint();
/// @brief Whether a full setpoint is due, either requested or because the last one is older than intervalMs
bool is_full_setpoint_due(std::uint32_t nowMs, std::uint32_t intervalMs) const;
void mark_full_setpoint_sent(std::uint32_t nowMs);

private:
isobus::DeviceDescriptorObjectPool pool; ///< The device descriptor object pool (DDOP) for the TC
Expand All @@ -79,6 +84,8 @@ class ClientState
bool isSectionControlEnabled = false; ///< Stores auto vs manual mode setting
bool usesPerElementControl = false; ///< Legacy mode: use per-element setpoint instead of condensed
std::uint16_t perElementSetpointDDI = 0; ///< The DDI to use for per-element setpoints (289 or 141), 0 if not applicable
bool fullSetpointRequested = false; ///< Send every section setpoint on the next update_section_setpoints()
std::uint32_t lastFullSetpointMs = 0; ///< When every section setpoint was last sent, 0 = never
};

// Create the task controller server object, this will handle all the ISOBUS communication for us
Expand Down Expand Up @@ -116,18 +123,35 @@ class MyTCServer : public isobus::TaskControllerServer
void update_section_states(std::vector<bool> &sectionStates);
void update_section_control_enabled(bool enabled);

/// @brief Keeps implements in automatic mode on AOG's section demand. Call from the update() thread.
/// @details Sending only on changes left an implement on its own state whenever AOG's demand did not
/// change: after switching to automatic, after the implement's master switch went on, and when AOG
/// sent no demand at all (no field open). This sends every setpoint right after those events and
/// every SECTION_SETPOINT_REFRESH_MS while in automatic, and treats a missing demand as all off.
void update_section_setpoints();

/// @brief Sends GNSS quality (DDI 514) to every client that declares that DDI.
/// @param quality NMEA 2000 GNSS Method: 0=No GNSS, 1=GNSS, 2=DGNSS, 3=Precise, 4=RTK Fixed, 5=RTK Float, 6=Estimated, 7=Manual, 8=Simulated
void send_gnss_quality(std::uint8_t quality);

private:
void send_section_setpoint_states(std::shared_ptr<isobus::ControlFunction> client, std::uint8_t ddiOffset);
/// @brief Copies AOG's section demand into a client's section setpoints
/// @returns A bit per condensed block (bit 0 = sections 1-16) whose setpoint changed
std::uint16_t apply_section_demand(ClientState &state);
void send_all_section_setpoint_states(std::shared_ptr<isobus::ControlFunction> client);
void send_section_setpoint_states(std::shared_ptr<isobus::ControlFunction> client, std::uint8_t ddiOffset, bool forceWorkState = false);
void send_section_control_state(std::shared_ptr<isobus::ControlFunction> client, bool enabled);
bool is_ddi_settable(std::shared_ptr<isobus::ControlFunction> client, std::uint16_t ddi);

std::map<std::shared_ptr<isobus::ControlFunction>, ClientState> clients;
std::map<std::shared_ptr<isobus::ControlFunction>, std::queue<std::vector<std::uint8_t>>> uploadedPools;

static constexpr std::uint32_t SECTION_SETPOINT_REFRESH_MS = 2000; ///< Resend interval while in automatic
static constexpr std::uint32_t SECTION_DEMAND_TIMEOUT_MS = 2000; ///< AOG sends PGN 229 every frame while a field is open
std::vector<bool> aogSectionDemand; ///< Last section states from AOG (PGN 229); all off when none is received
std::uint32_t lastSectionDemandMs = 0; ///< When AOG's section demand was last received, 0 = never
bool sectionDemandReceived = false; ///< Whether AOG's section demand is currently arriving

/// @brief Guards clients and uploadedPools.
///
/// CONCURRENCY: the isobus/AgIsoStack stack runs its own background thread
Expand Down
1 change: 1 addition & 0 deletions src/app.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -629,6 +629,7 @@ bool Application::update()
tcServer->request_measurement_commands();
warn_if_late("tcServer->update()", lastTcServerUpdateCallMs, 200);
tcServer->update();
tcServer->update_section_setpoints();
if (tcFunctionalities)
tcFunctionalities->update();
if (tecuFunctionalities)
Expand Down
163 changes: 132 additions & 31 deletions src/task_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,9 @@

#include "isobus/isobus/isobus_device_descriptor_object_pool_helpers.hpp"
#include "isobus/isobus/isobus_task_controller_server.hpp"
#include "isobus/utility/system_timing.hpp"

#include <algorithm>
#include <bitset>
#include <filesystem>
#include <fstream>
Expand Down Expand Up @@ -199,6 +201,22 @@ std::uint8_t ClientState::get_section_actual_state(std::uint8_t section) const
return SectionState::NOT_INSTALLED;
}

void ClientState::request_full_setpoint()
{
fullSetpointRequested = true;
}

bool ClientState::is_full_setpoint_due(std::uint32_t nowMs, std::uint32_t intervalMs) const
{
return fullSetpointRequested || (0 == lastFullSetpointMs) || ((nowMs - lastFullSetpointMs) >= intervalMs);
}

void ClientState::mark_full_setpoint_sent(std::uint32_t nowMs)
{
fullSetpointRequested = false;
lastFullSetpointMs = (0 == nowMs) ? 1 : nowMs;
}

bool ClientState::is_any_section_setpoint_on() const
{
for (std::uint8_t state : sectionSetpointStates)
Expand Down Expand Up @@ -760,14 +778,30 @@ bool MyTCServer::on_value_command(std::shared_ptr<isobus::ControlFunction> partn

case static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SectionControlState):
{
clients[partner].set_section_control_enabled(processDataValue == 1);
const bool enabled = (processDataValue == 1);
if (enabled && !clients[partner].is_section_control_enabled())
{
// The implement went to automatic (possibly from its own terminal): give it our setpoint now
clients[partner].request_full_setpoint();
}
clients[partner].set_section_control_enabled(enabled);
}
break;

case static_cast<std::uint16_t>(isobus::DataDescriptionIndex::ActualWorkState):
{
// Master switch on: some implements then turn every section on until told otherwise
bool wasWorking = false;
const bool isWorking = (processDataValue == 1);
if (isWorking &&
(!clients[partner].try_get_element_work_state(elementNumber, wasWorking) || !wasWorking) &&
clients[partner].is_section_control_enabled())
{
clients[partner].request_full_setpoint();
}

// Store the work state per element (used for parent-off checks)
clients[partner].set_element_work_state(elementNumber, processDataValue == 1);
clients[partner].set_element_work_state(elementNumber, isWorking);

// For legacy per-element devices only: propagate to section actual states
// so the heartbeat (PGN 0xF0) can report them to AOG.
Expand Down Expand Up @@ -948,46 +982,39 @@ void MyTCServer::request_measurement_commands()
void MyTCServer::update_section_states(std::vector<bool> &sectionStates)
{
std::lock_guard<std::recursive_mutex> lock(clientsMutex);
aogSectionDemand = sectionStates;
lastSectionDemandMs = isobus::SystemTiming::get_timestamp_ms();
if (0 == lastSectionDemandMs)
{
lastSectionDemandMs = 1; // 0 means "never"
}

for (auto &client : clients)
{
auto &state = client.second;
if (!state.is_section_control_enabled())
{
// According to standard, the section setpoint states should only be sent when in auto mode
continue;
}

// Skip clients that don't have any sections configured (e.g., tractors or other non-implement devices)
if (state.get_number_of_sections() == 0)
{
continue;
}

bool requiresUpdate = false;
for (std::uint8_t i = 0; i < state.get_number_of_sections(); i++)
// Keep the setpoints current in manual mode too, so switching to automatic can send them right away
const std::uint16_t changedBlocks = apply_section_demand(state);

if (!state.is_section_control_enabled())
{
if ((i % NUMBER_SECTIONS_PER_CONDENSED_MESSAGE == 0) && requiresUpdate)
{
// Send the previous 16 sections
std::uint8_t ddiOffset = (i / NUMBER_SECTIONS_PER_CONDENSED_MESSAGE) - 1;
send_section_setpoint_states(client.first, ddiOffset);
requiresUpdate = false;
}
// According to standard, the section setpoint states should only be sent when in auto mode
continue;
}

if (i < sectionStates.size())
for (std::uint8_t block = 0; block * NUMBER_SECTIONS_PER_CONDENSED_MESSAGE < state.get_number_of_sections(); block++)
{
if (0 != (changedBlocks & (1u << block)))
{
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;
}
send_section_setpoint_states(client.first, block);
}
}
if (requiresUpdate)
{
std::uint8_t ddiOffset = (state.get_number_of_sections() - 1) / NUMBER_SECTIONS_PER_CONDENSED_MESSAGE;
send_section_setpoint_states(client.first, ddiOffset);
}
}
}

Expand All @@ -1008,6 +1035,80 @@ void MyTCServer::update_section_control_enabled(bool enabled)
{
send_section_control_state(client.first, enabled);
}

if (enabled)
{
// Without this the implement runs on its own state until AOG's demand next changes
client.second.request_full_setpoint();
}
}
}

void MyTCServer::update_section_setpoints()
{
std::lock_guard<std::recursive_mutex> lock(clientsMutex);
const std::uint32_t nowMs = isobus::SystemTiming::get_timestamp_ms();

const bool demandReceived = (0 != lastSectionDemandMs) && !isobus::SystemTiming::time_expired_ms(lastSectionDemandMs, SECTION_DEMAND_TIMEOUT_MS);
if (demandReceived != sectionDemandReceived)
{
sectionDemandReceived = demandReceived;
if (demandReceived)
{
log("TC Server") << "Receiving section states from AOG (PGN 229)" << std::endl;
}
else
{
// AOG stops sending PGN 229 when no field is open. The last demand may have had sections on.
log("TC Server") << "No section states from AOG (PGN 229) for " << SECTION_DEMAND_TIMEOUT_MS
<< " ms (no field open?): commanding all sections off" << std::endl;
std::fill(aogSectionDemand.begin(), aogSectionDemand.end(), false);
for (auto &client : clients)
{
apply_section_demand(client.second);
client.second.request_full_setpoint();
}
}
}

for (auto &client : clients)
{
auto &state = client.second;
if ((0 == state.get_number_of_sections()) || !state.is_section_control_enabled())
{
continue;
}

if (state.is_full_setpoint_due(nowMs, SECTION_SETPOINT_REFRESH_MS))
{
send_all_section_setpoint_states(client.first);
state.mark_full_setpoint_sent(nowMs);
}
}
}

std::uint16_t MyTCServer::apply_section_demand(ClientState &state)
{
std::uint16_t changedBlocks = 0;
for (std::uint8_t i = 0; i < state.get_number_of_sections(); i++)
{
const bool on = (i < aogSectionDemand.size()) && aogSectionDemand[i];
if (on != (state.get_section_setpoint_state(i) == SectionState::ON))
{
state.set_section_setpoint_state(i, on ? SectionState::ON : SectionState::OFF);
changedBlocks |= static_cast<std::uint16_t>(1u << (i / NUMBER_SECTIONS_PER_CONDENSED_MESSAGE));
}
}
return changedBlocks;
}

void MyTCServer::send_all_section_setpoint_states(std::shared_ptr<isobus::ControlFunction> client)
{
std::lock_guard<std::recursive_mutex> lock(clientsMutex);
const std::uint8_t numberOfSections = clients[client].get_number_of_sections();
for (std::uint8_t block = 0; block * NUMBER_SECTIONS_PER_CONDENSED_MESSAGE < numberOfSections; block++)
{
send_section_setpoint_states(client, block, true);
}
}

Expand All @@ -1024,7 +1125,7 @@ void MyTCServer::send_gnss_quality(std::uint8_t quality)
}
}

void MyTCServer::send_section_setpoint_states(std::shared_ptr<isobus::ControlFunction> client, std::uint8_t ddiOffset)
void MyTCServer::send_section_setpoint_states(std::shared_ptr<isobus::ControlFunction> client, std::uint8_t ddiOffset, bool forceWorkState)
{
std::lock_guard<std::recursive_mutex> lock(clientsMutex);
std::uint8_t sectionOffset = ddiOffset * NUMBER_SECTIONS_PER_CONDENSED_MESSAGE;
Expand All @@ -1044,12 +1145,12 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr<isobus::ControlFun
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))
if ((forceWorkState || (clients[client].get_setpoint_work_state() != setpointWorkState)) && clients[client].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState))
{
send_set_value(client, static_cast<std::uint16_t>(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))
else if (!forceWorkState && !clients[client].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState))
{
log("TC Server") << "DDI 289 (SetpointWorkState) not available!" << std::endl;
}
Expand Down Expand Up @@ -1088,7 +1189,7 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr<isobus::ControlFun

// Also send global work state on the boom/device element if DDI 289 is available
bool setpointWorkState = clients[client].is_any_section_setpoint_on();
if (clients[client].get_setpoint_work_state() != setpointWorkState)
if (forceWorkState || (clients[client].get_setpoint_work_state() != setpointWorkState))
{
if (clients[client].has_element_number_for_ddi(isobus::DataDescriptionIndex::SetpointWorkState))
{
Expand Down
Loading