Skip to content

Add wheel and ground speed #8

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’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
GwnDaan merged 2 commits into from
Mar 15, 2025
Merged

Add wheel and ground speed #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3e6b574
feat: add wheel and ground speed
GwnDaan Mar 15, 2025
89bf8ad
Add process variables for exchanging data
GwnDaan Mar 14, 2025
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
4 changes: 2 additions & 2 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 3.16)
project("AOG-TaskController")
set(PROJECT_VERSION_MAJOR 1)
set(PROJECT_VERSION_MINOR 0)
set(PROJECT_VERSION_MINOR 1)
set(PROJECT_VERSION_PATCH 0)
set(PROJECT_VERSION
"${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}.${PROJECT_VERSION_PATCH}"
Expand Down Expand Up @@ -29,7 +29,7 @@ FetchContent_MakeAvailable(Boost)
FetchContent_Declare(
isobus
GIT_REPOSITORY https://github.com/Open-Agriculture/AgIsoStack-plus-plus.git
GIT_TAG 58686f401282d34b8f15a704a82798222083ca3f
GIT_TAG 5f71ceae594f751d1befc775fa01e38a3ba0f5d3
DOWNLOAD_EXTRACT_TIMESTAMP TRUE)
FetchContent_MakeAvailable(isobus)

Expand Down
109 changes: 69 additions & 40 deletions src/app.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include "isobus/hardware_integration/can_hardware_interface.hpp"
#include "isobus/isobus/can_network_manager.hpp"
#include "isobus/isobus/isobus_preferred_addresses.hpp"
#include "isobus/isobus/isobus_standard_data_description_indices.hpp"
#include "isobus/utility/system_timing.hpp"

#include "task_controller.hpp"
Expand Down Expand Up @@ -73,11 +74,16 @@ bool Application::initialize()
tcServer->initialize();

// Initialize speed and distance messages
speedMessagesInterface = std::make_unique<isobus::SpeedMessagesInterface>(serverCF, false, false, true, false);
speedMessagesInterface = std::make_unique<isobus::SpeedMessagesInterface>(serverCF, true, true, true, false); //TODO: make configurable whether to send these messages
speedMessagesInterface->initialize();
nmea2000MessageInterface = std::make_unique<isobus::NMEA2000MessageInterface>(serverCF, false, false, false, false, false, false, false);
nmea2000MessageInterface->initialize();
nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(true);
nmea2000MessageInterface->set_enable_sending_cog_sog_cyclically(true); // TODO: make configurable whether to send these messages

speedMessagesInterface->wheelBasedSpeedTransmitData.set_implement_start_stop_operations_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::ImplementStartStopOperations::NotAvailable);
speedMessagesInterface->wheelBasedSpeedTransmitData.set_key_switch_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::KeySwitchState::NotAvailable);
speedMessagesInterface->wheelBasedSpeedTransmitData.set_operator_direction_reversed_state(isobus::SpeedMessagesInterface::WheelBasedMachineSpeedData::OperatorDirectionReversed::NotAvailable);
speedMessagesInterface->machineSelectedSpeedTransmitData.set_speed_source(isobus::SpeedMessagesInterface::MachineSelectedSpeedData::SpeedSource::NavigationBasedSpeed);

std::cout << "Task controller server started." << std::endl;

Expand All @@ -87,44 +93,6 @@ bool Application::initialize()
auto packetHandler = [this, serverCF](std::uint8_t src, std::uint8_t pgn, std::span<std::uint8_t> data) {
if (src == 0x7F && pgn == 0xFE) // 254 - Steer Data
{
std::uint16_t speed = data[0] | (data[1] << 8);
std::uint8_t status = data[2];

// Convert from hm/h to mm/s
speed *= 1000 / 36;

speedMessagesInterface->machineSelectedSpeedTransmitData.set_speed_source(isobus::SpeedMessagesInterface::MachineSelectedSpeedData::SpeedSource::NavigationBasedSpeed);
speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_direction_of_travel(isobus::SpeedMessagesInterface::MachineDirection::Forward); // TODO: Implement direction
speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_speed(speed);
speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance
auto &cog_sog_message = nmea2000MessageInterface->get_cog_sog_transmit_message();
cog_sog_message.set_sequence_id(nmea2000SequenceIdentifier++);
cog_sog_message.set_speed_over_ground(speed);
cog_sog_message.set_course_over_ground(0); // TODO: Implement course
cog_sog_message.set_course_over_ground_reference(isobus::NMEA2000Messages::CourseOverGroundSpeedOverGroundRapidUpdate::CourseOverGroundReference::NotApplicableOrNull);

std::int32_t xte = (data[5] - 127) * 2;
static const std::uint8_t xteMode = 0b00000001;
xteSid = xteSid % 253 + 1;

std::array<std::uint8_t, 8> xteData = {
xteSid, // Sequence ID
static_cast<std::uint8_t>(xteMode | 0b00110000 | (status == 1 ? 0b00000000 : 0b01000000)), // XTE mode (4 bits) + Reserved (2 bits set to 1) + Navigation Terminated (2 bits)
static_cast<std::uint8_t>(xte & 0xFF), // XTE LSB
static_cast<std::uint8_t>((xte >> 8) & 0xFF), // XTE
static_cast<std::uint8_t>((xte >> 16) & 0xFF), // XTE
static_cast<std::uint8_t>((xte >> 24) & 0xFF), // XTE MSB
0xFF, // Reserved byte 1 (all bits set to 1)
0xFF // Reserved byte 2 (all bits set to 1)
};
if (isobus::SystemTiming::time_expired_ms(lastXteTransmit, 1000)) // Transmit every second
{
if (isobus::CANNetworkManager::CANNetwork.send_can_message(0x1F903, xteData.data(), xteData.size(), serverCF))
{
lastXteTransmit = isobus::SystemTiming::get_timestamp_ms();
}
}

// TODO: hack to get desired section states. probably want to make a new pgn later when we need more than 16 sections
std::vector<bool> sectionStates;
for (std::uint8_t i = 0; i < 8; i++)
Expand All @@ -144,6 +112,67 @@ bool Application::initialize()
std::cout << "Received request from AOG to change section control state to " << (sectionControlState == 1 ? "enabled" : "disabled") << std::endl;
tcServer->update_section_control_enabled(sectionControlState == 1);
}
else if (src == 0x7F && pgn == 0xF2) // Process Data
{
auto identifier = static_cast<isobus::DataDescriptionIndex>(data[0] | (data[1] << 8));

std::int32_t value = data[2] | (data[3] << 8) | (data[4] << 16) | (data[5] << 24);
if (identifier == isobus::DataDescriptionIndex::ActualSpeed)
{
std::uint16_t speed = std::abs(value);
auto direction = value < 0 ? isobus::SpeedMessagesInterface::MachineDirection::Reverse : isobus::SpeedMessagesInterface::MachineDirection::Forward;
speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_direction_of_travel(direction);
speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_direction_of_travel(direction);
speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_direction_of_travel(direction);

speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_speed(speed);
speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_speed(speed);
speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_speed(speed);

speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance
speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance
speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_distance(0); // TODO: Implement distance

auto &cog_sog_message = nmea2000MessageInterface->get_cog_sog_transmit_message();
cog_sog_message.set_sequence_id(nmea2000SequenceIdentifier++);
cog_sog_message.set_speed_over_ground(speed);
cog_sog_message.set_course_over_ground(0); // TODO: Implement course
cog_sog_message.set_course_over_ground_reference(isobus::NMEA2000Messages::CourseOverGroundSpeedOverGroundRapidUpdate::CourseOverGroundReference::NotApplicableOrNull);
}
else if (identifier == isobus::DataDescriptionIndex::GuidanceLineDeviation)
{
std::int32_t xte = value / 1000; // Convert from mm to m
static const std::uint8_t xteMode = 0b00000001;
xteSid = xteSid % 253 + 1;

std::uint8_t status = 0; // TODO: navigation terminated status

std::array<std::uint8_t, 8> xteData = {
xteSid, // Sequence ID
static_cast<std::uint8_t>(xteMode | 0b00110000 | (status == 1 ? 0b00000000 : 0b01000000)), // XTE mode (4 bits) + Reserved (2 bits set to 1) + Navigation Terminated (2 bits)
static_cast<std::uint8_t>(xte & 0xFF), // XTE LSB
static_cast<std::uint8_t>((xte >> 8) & 0xFF), // XTE
static_cast<std::uint8_t>((xte >> 16) & 0xFF), // XTE
static_cast<std::uint8_t>((xte >> 24) & 0xFF), // XTE MSB
0xFF, // Reserved byte 1 (all bits set to 1)
0xFF // Reserved byte 2 (all bits set to 1)
};
if (isobus::SystemTiming::time_expired_ms(lastXteTransmit, 1000)) // Transmit every second
{
if (isobus::CANNetworkManager::CANNetwork.send_can_message(0x1F903, xteData.data(), xteData.size(), serverCF))
{
lastXteTransmit = isobus::SystemTiming::get_timestamp_ms();
}
}
}
else if (static_cast<std::uint16_t>(identifier) == 597 /*isobus::DataDescriptionIndex::TotalDistance*/)
{
auto distance = static_cast<std::uint32_t>(value);
speedMessagesInterface->groundBasedSpeedTransmitData.set_machine_distance(distance);
speedMessagesInterface->wheelBasedSpeedTransmitData.set_machine_distance(distance);
speedMessagesInterface->machineSelectedSpeedTransmitData.set_machine_distance(distance);
}
}
};
udpConnections->set_packet_handler(packetHandler);
udpConnections->open();
Expand Down
21 changes: 0 additions & 21 deletions src/task_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -331,26 +331,17 @@ bool MyTCServer::on_value_command(std::shared_ptr<isobus::ControlFunction> partn
{
clients[partner].set_section_actual_state(i + sectionIndexOffset, (processDataValue >> (2 * i)) & 0x03);
}

std::cout << "Received actual condensed work state for element number " << int(elementNumber) << " and DDI " << int(dataDescriptionIndex) << " with states: ";
for (std::uint8_t i = 0; i < NUMBER_SECTIONS_PER_CONDENSED_MESSAGE; i++)
{
std::cout << static_cast<int>(clients[partner].get_section_actual_state(sectionIndexOffset + i)) << " ";
}
std::cout << std::endl;
}
break;

case static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SectionControlState):
{
std::cout << "Received section control state: " << processDataValue << std::endl;
clients[partner].set_section_control_enabled(processDataValue == 1);
}
break;

case static_cast<std::uint16_t>(isobus::DataDescriptionIndex::ActualWorkState):
{
std::cout << "Received actual work state: " << processDataValue << std::endl;
clients[partner].set_setpoint_work_state(processDataValue == 1);
}
}
Expand Down Expand Up @@ -406,12 +397,10 @@ void MyTCServer::request_measurement_commands()

if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::OnChange))
{
std::cout << "Requesting on-change trigger for element number " << int(elementObject->get_element_number()) << " and DDI " << int(processDataObject->get_ddi()) << std::endl;
send_change_threshold_measurement_command(client.first, processDataObject->get_ddi(), elementObject->get_element_number(), 1);
}
if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::TimeInterval))
{
std::cout << "Requesting time interval trigger for element number " << int(elementObject->get_element_number()) << " and DDI " << int(processDataObject->get_ddi()) << std::endl;
send_time_interval_measurement_command(client.first, processDataObject->get_ddi(), elementObject->get_element_number(), 1000);
}
}
Expand Down Expand Up @@ -450,7 +439,6 @@ void MyTCServer::request_measurement_commands()

if (processDataObject->has_trigger_method(isobus::task_controller_object::DeviceProcessDataObject::AvailableTriggerMethods::OnChange))
{
std::cout << "Requesting on-change trigger for element number " << int(elementObject->get_element_number()) << " and DDI " << int(processDataObject->get_ddi()) << std::endl;
send_change_threshold_measurement_command(client.first, processDataObject->get_ddi(), elementObject->get_element_number(), 1);
}
}
Expand Down Expand Up @@ -527,28 +515,19 @@ void MyTCServer::send_section_setpoint_states(std::shared_ptr<isobus::ControlFun
value |= (clients[client].get_section_setpoint_state(sectionOffset + i) << (2 * i));
}

std::cout << "Sending setpoint states for DDI offset " << static_cast<int>(ddiOffset) << " with states: ";
for (std::uint8_t i = 0; i < NUMBER_SECTIONS_PER_CONDENSED_MESSAGE; i++)
{
std::cout << static_cast<int>(clients[client].get_section_setpoint_state(sectionOffset + i)) << " ";
}
std::cout << std::endl;

std::uint16_t ddiTarget = static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SetpointCondensedWorkState1_16) + ddiOffset;
std::uint16_t elementNumber = clients[client].get_element_number_for_ddi(static_cast<isobus::DataDescriptionIndex>(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))
{
std::cout << "Sending setpoint work state: " << (setpointWorkState ? "on" : "off") << std::endl;
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);
}
}

void MyTCServer::send_section_control_state(std::shared_ptr<isobus::ControlFunction> client, bool enabled)
{
std::cout << "Sending section control state: " << (enabled ? "enabled" : "disabled") << std::endl;
send_set_value(client, static_cast<std::uint16_t>(isobus::DataDescriptionIndex::SectionControlState), clients[client].get_element_number_for_ddi(isobus::DataDescriptionIndex::SectionControlState), enabled ? 1 : 0);
}