From fd30bdd7d325feb0fd51c363a4596b98bce814f9 Mon Sep 17 00:00:00 2001 From: akemiyu <43460886+akemiyu@users.noreply.github.com> Date: Tue, 16 Dec 2025 03:11:21 -0500 Subject: [PATCH 1/8] Create VectorNav --- drivebrain_comms/include/VNComms.hpp | 75 ++++++++++ drivebrain_comms/src/VNComms.cpp | 211 +++++++++++++++++++++++++++ tests/test_vn.cpp | 170 +++++++++++++++++++++ 3 files changed, 456 insertions(+) create mode 100644 drivebrain_comms/include/VNComms.hpp create mode 100644 drivebrain_comms/src/VNComms.cpp create mode 100644 tests/test_vn.cpp diff --git a/drivebrain_comms/include/VNComms.hpp b/drivebrain_comms/include/VNComms.hpp new file mode 100644 index 0000000..547bdef --- /dev/null +++ b/drivebrain_comms/include/VNComms.hpp @@ -0,0 +1,75 @@ +#pragma once + +#include +#include +#include + +// protobuf +#include +#include +#include +#include "hytech_msgs.pb.h" + +// boost +#include +#include + +// c++ stl includes +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "libvncxx/vntime.h" +#include "libvncxx/packetfinder.h" +#include "libvncxx/packet.h" + +using namespace vn::xplat; +using namespace vn::protocol::uart; +using SerialPort = boost::asio::serial_port; +using loggertype = core::MsgLogger>; + +namespace comms { + class VNDriver : public core::common::Loggable>, + public core::common::Configurable + { + public: + VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr state_estimator, boost::asio::io_context &io_context, bool &init_successful); + ~VNDriver() { + spdlog::info("destructed %s", this->get_name()); + } + bool init(); + struct config { + int baud_rate; + int freq_divisor; + }; + + private: + std::shared_ptr _state_estimator; + + + vn::protocol::uart::PacketFinder _processor; + boost::array _output_buff; + boost::array _input_buff; + SerialPort _serial; + config _config; + + public: + // Public methods + void log_proto_message(std::shared_ptr msg); + + private: + // Private methods + static void _handle_recieve(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts); + void _configure_binary_outputs(); + void _start_recieve(); + void _set_initial_heading(); + }; +} \ No newline at end of file diff --git a/drivebrain_comms/src/VNComms.cpp b/drivebrain_comms/src/VNComms.cpp new file mode 100644 index 0000000..39afdb3 --- /dev/null +++ b/drivebrain_comms/src/VNComms.cpp @@ -0,0 +1,211 @@ +#include + +// standard includes +#include +#include +#include +#include + +#include "hytech_msgs.pb.h" +#include "base_msgs.pb.h" + +#include "libvncxx/vntime.h" +#include "libvncxx/packetfinder.h" +#include "libvncxx/packet.h" +#include + +namespace comms +{ + + bool VNDriver::init() + { + // Try to establish a connection to the driver + spdlog::info("Opening vn driver."); + auto device_name = get_parameter_value("device_name"); + _config.baud_rate = get_parameter_value("baud_rate").value(); + _config.freq_divisor = get_parameter_value("freq_divisor").value(); + auto port = get_parameter_value("port"); + + _processor.registerPossiblePacketFoundHandler(this, &VNDriver::_handle_recieve); + + boost::system::error_code ec; + + auto ec_ret = _serial.open(device_name.value(), ec); + + if (ec) + { + spdlog::warn("Error: {}", ec.message()); + spdlog::info("failed to open vectornav serial port"); + return false; + } + + // Set the baud rate of the device along with other configs + _serial.set_option(SerialPort::baud_rate(_config.baud_rate)); + _serial.set_option(SerialPort::character_size(8)); + _serial.set_option(SerialPort::parity(SerialPort::parity::none)); + _serial.set_option(SerialPort::stop_bits(SerialPort::stop_bits::one)); + _serial.set_option(SerialPort::flow_control(SerialPort::flow_control::none)); + + // Configures the binary outputs for the device + spdlog::info("Configuring binary outputs."); + _configure_binary_outputs(); + + set_configured(); + return true; + } + + VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr state_estimator, boost::asio::io_context& io, bool &init_not_successful) + : core::common::Configurable(json_file_handler, "VNDriver"), + _state_estimator(state_estimator), + _serial(io) + { + init_not_successful = !init(); + + // Starts read + if(!init_not_successful) + { + spdlog::info("Starting vn driver recieve."); + _start_recieve(); + } + + } + + void VNDriver::log_proto_message(std::shared_ptr msg) + { + _state_estimator->handle_recv_process(static_cast>(msg)); + this->log(msg); + } + + void VNDriver::_configure_binary_outputs() + { + + auto num_of_bytes = Packet::genWriteBinaryOutput1( + ErrorDetectionMode::ERRORDETECTIONMODE_NONE, + (char *)_output_buff.data(), + _output_buff.size(), + AsyncMode::ASYNCMODE_PORT1, + _config.freq_divisor, + (CommonGroup::COMMONGROUP_YAWPITCHROLL | CommonGroup::COMMONGROUP_ANGULARRATE), // Note use of binary OR to configure flags. + TimeGroup::TIMEGROUP_NONE, + ImuGroup::IMUGROUP_UNCOMPACCEL, + GpsGroup::GPSGROUP_NONE, + AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY, + (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY | InsGroup::INSGROUP_VELU), + GpsGroup::GPSGROUP_NONE); + + boost::asio::async_write(_serial, + boost::asio::buffer(_output_buff.data(), num_of_bytes), + [](const boost::system::error_code &ec, std::size_t bytes_transferred) + { + if (!ec) + { + spdlog::warn("Successfully sent {} bytes.", bytes_transferred); + } + else + { + spdlog::error("Error sending data: {}", ec.message()); + } + }); + } + + void VNDriver::_handle_recieve(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts) + { + auto this_instance = (VNDriver *)userData; + if (packet.type() == vn::protocol::uart::Packet::TYPE_BINARY) + { + vn::math::vec3f vel; + // See if this is a binary packet type we are expecting. + if (!packet.isCompatible((CommonGroup::COMMONGROUP_YAWPITCHROLL | CommonGroup::COMMONGROUP_ANGULARRATE), // Note use of binary OR to configure flags. + TimeGroup::TIMEGROUP_NONE, + ImuGroup::IMUGROUP_UNCOMPACCEL, + GpsGroup::GPSGROUP_NONE, + AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY, + (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY | InsGroup::INSGROUP_VELU), + GpsGroup::GPSGROUP_NONE)) + { + spdlog::warn("ERROR: packet is not what we want"); + return; + } + + // Extract data in correct order + auto ypr_data = packet.extractVec3f(); + auto angular_rate_data = packet.extractVec3f(); + auto uncomp_accel = packet.extractVec3f(); + auto linear_accel_body = packet.extractVec3f(); + uint16_t ins_status = packet.extractUint16(); + auto pos_lla = packet.extractVec3d(); + auto vel_body = packet.extractVec3f(); + auto vel_uncertainty = packet.extractFloat(); + + // Create the protobuf message to send + std::shared_ptr msg_out = std::make_shared(); + + hytech_msgs::xyz_vector *linear_vel_msg = msg_out->mutable_vn_vel_m_s(); + linear_vel_msg->set_x(vel_body.x); + linear_vel_msg->set_y(vel_body.y); + linear_vel_msg->set_z(vel_body.z); + + hytech_msgs::xyz_vector *linear_accel_msg = msg_out->mutable_vn_linear_accel_m_ss(); + linear_accel_msg->set_x(linear_accel_body.x); + linear_accel_msg->set_y(linear_accel_body.y); + linear_accel_msg->set_z(linear_accel_body.z); + + hytech_msgs::xyz_vector *linear_accel_uncomp_msg = msg_out->mutable_vn_linear_accel_uncomp_m_ss(); + linear_accel_uncomp_msg->set_x(uncomp_accel.x); + linear_accel_uncomp_msg->set_y(uncomp_accel.y); + linear_accel_uncomp_msg->set_z(uncomp_accel.z); + + hytech_msgs::xyz_vector *angular_rate_data_msg = msg_out->mutable_vn_angular_rate_rad_s(); + angular_rate_data_msg->set_x(angular_rate_data.x); + angular_rate_data_msg->set_y(angular_rate_data.y); + angular_rate_data_msg->set_z(angular_rate_data.z); + + hytech_msgs::ypr_vector *ypr_data_msg = msg_out->mutable_vn_ypr_rad(); + ypr_data_msg->set_yaw(ypr_data.x); + ypr_data_msg->set_pitch(ypr_data.y); + ypr_data_msg->set_roll(ypr_data.z); + + hytech_msgs::GPS_data *vn_gps_msg = msg_out->mutable_vn_gps(); + vn_gps_msg->set_lat(pos_lla.x); + vn_gps_msg->set_lon(pos_lla.y); + + hytech_msgs::vn_status *vn_ins_msg = msg_out->mutable_status(); + vn_ins_msg->set_ins_mode(static_cast(ins_status & 0b11)); + vn_ins_msg->set_gnss_fix((ins_status >> 2) & 0b1); + vn_ins_msg->set_error_imu((ins_status >> 4) & 1); + vn_ins_msg->set_error_mag_pres((ins_status >> 5) & 0b1); + vn_ins_msg->set_error_gnss((ins_status >> 6) & 0b1); + vn_ins_msg->set_gnss_heading_ins((ins_status >> 8) & 0b1); + vn_ins_msg->set_gnss_compass((ins_status >> 9) & 0b1); + vn_ins_msg->set_ins_mode_int((ins_status & 0b11)); + vn_ins_msg->set_ins_vel_u(vel_uncertainty); + + this_instance->log_proto_message(static_cast>(msg_out)); + } + else + { + spdlog::warn("Packet not correct"); + } + } + + void VNDriver::_start_recieve() + { + _serial.async_read_some( + boost::asio::buffer(_input_buff), + [&](const boost::system::error_code &ec, std::size_t bytesCount) -> void + { + if (ec) + { + if (ec != boost::asio::error::operation_aborted) + { + spdlog::error("ERROR: {}", ec.message()); + } + return; + } + // _logger.log_string("logging", core::LogLevel::INFO); + _processor.processReceivedData((char *)(_input_buff.data()), bytesCount); + // Initiate another asynchronous read + _start_recieve(); + }); + } +} diff --git a/tests/test_vn.cpp b/tests/test_vn.cpp new file mode 100644 index 0000000..d4b2ca8 --- /dev/null +++ b/tests/test_vn.cpp @@ -0,0 +1,170 @@ +#include +#include +#include +#include "libvncxx/vntime.h" +#include "libvncxx/packetfinder.h" +#include "libvncxx/packet.h" +#include +#include +#include +#include + +// #include +#include // std::chrono::seconds + +using SerialPort = boost::asio::serial_port; + +using namespace vn::xplat; +using namespace vn::protocol::uart; + +std::chrono::time_point init = {}; + +void validPacketFoundHandler(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts) +{ + if (packet.type() == vn::protocol::uart::Packet::TYPE_BINARY) + { + vn::math::vec3f vel; + // See if this is a binary packet type we are expecting. + if (!packet.isCompatible((CommonGroup::COMMONGROUP_YAWPITCHROLL | CommonGroup::COMMONGROUP_ANGULARRATE), // Note use of binary OR to configure flags. + TimeGroup::TIMEGROUP_NONE, + ImuGroup::IMUGROUP_UNCOMPACCEL, + GpsGroup::GPSGROUP_NONE, + AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY, + (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY), + GpsGroup::GPSGROUP_NONE)) + { + // Not the type of binary packet we are expecting. + std::cout << "ERROR: packet is not what we want" << std::endl; + return; + } + + // Ok, we have our expected binary output packet. Since there are many + // ways to configure the binary data output, the burden is on the user + // to correctly parse the binary packet. However, we can make use of + // the parsing convenience methods provided by the VnUartPacket structure. + // When using these convenience methods, you have to extract them in + // the order they are organized in the binary packet per the User Manual. + auto ypr_data = packet.extractVec3f(); + auto angular_rate_data = packet.extractVec3f(); + auto uncomp_accel = packet.extractVec3f(); + auto linear_accel_body = packet.extractVec3f(); + uint16_t ins_status = packet.extractUint16(); + auto pos_lla = packet.extractVec3d(); + auto vel_body = packet.extractVec3f(); + + std::cout << "got data" << std::endl; + std::cout << "ypr " << ypr_data.x << " " << ypr_data.y << " " << ypr_data.z << std::endl; + std::cout << "uncomp_accel " << uncomp_accel.x << " " << uncomp_accel.y << " " << uncomp_accel.z << std::endl; + std::cout << "linear_accel_body " << linear_accel_body.x << " " << linear_accel_body.y << " " << linear_accel_body.z << std::endl; + std::cout << "ins_status " << static_cast(ins_status &= 0x3) << std::endl; + std::cout << "pos_lla " << pos_lla.x << " " << pos_lla.y << " " << pos_lla.z << std::endl; + std::cout << "vel_body " << vel_body.x << " " << vel_body.y << " " << vel_body.z << std::endl; + + auto curr = std::chrono::high_resolution_clock::now(); + + float millis = std::chrono::duration_cast(curr - init).count(); + + std::cout << "dt ms: " << millis << std::endl; + init = curr; + } + else + { + std::cout << "packet not correct" << std::endl; + } +} + +void readData(SerialPort &m_serial, boost::array &m_inputBuf, vn::protocol::uart::PacketFinder &processor) +{ + m_serial.async_read_some( + boost::asio::buffer(m_inputBuf), + [&](const boost::system::error_code &ec, std::size_t bytesCount) + { + if (ec) + { + if (ec != boost::asio::error::operation_aborted) + { + std::cerr << "ERROR: " << ec.message() << std::endl; + } + return; + } + + processor.processReceivedData((char *)(m_inputBuf.data()), bytesCount); + // Initiate another asynchronous read + readData(m_serial, m_inputBuf, processor); + }); +} + +void write_data(SerialPort &m_serial, boost::array &m_outputBuf) +{ + boost::array m_inputBuf; + + // Generate binary output data to write + auto numOfBytes = Packet::genWriteBinaryOutput1( + ErrorDetectionMode::ERRORDETECTIONMODE_NONE, + (char *)m_outputBuf.data(), + m_outputBuf.size(), + AsyncMode::ASYNCMODE_PORT1, + 1, + (CommonGroup::COMMONGROUP_YAWPITCHROLL | CommonGroup::COMMONGROUP_ANGULARRATE), // Binary OR for flags. + TimeGroup::TIMEGROUP_NONE, + ImuGroup::IMUGROUP_UNCOMPACCEL, + GpsGroup::GPSGROUP_NONE, + AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY, + (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY), + GpsGroup::GPSGROUP_NONE); + + // Perform another blocking write + try + { + std::size_t bytes_written = boost::asio::write(m_serial, boost::asio::buffer(m_outputBuf.data(), numOfBytes)); + std::cout << "Successfully sent " << bytes_written << " bytes.\n"; + } + catch (const boost::system::system_error &e) + { + std::cerr << "Error sending data: " << e.what() << "\n"; + } + + // Perform another blocking read + try + { + std::size_t bytes_read = m_serial.read_some(boost::asio::buffer(m_inputBuf)); + // Process the received data + // processor.processReceivedData((char *)(m_inputBuf.data()), bytes_read); + std::cout <<"read"< m_inputBuf; + boost::array m_oututBuf; + + write_data(m_serial, m_oututBuf); + // processor.resetTracking(); + readData(m_serial, m_inputBuf, processor); + io.run(); + return 0; +} \ No newline at end of file From da990000dbc83d45d2aab87312d3185ab1c88d80 Mon Sep 17 00:00:00 2001 From: akemiyu <43460886+akemiyu@users.noreply.github.com> Date: Sun, 11 Jan 2026 22:28:39 -0800 Subject: [PATCH 2/8] Changed method bracket style --- drivebrain_comms/src/VNComms.cpp | 50 ++++++++++---------------------- tests/test_vn.cpp | 48 ++++++++++-------------------- 2 files changed, 31 insertions(+), 67 deletions(-) diff --git a/drivebrain_comms/src/VNComms.cpp b/drivebrain_comms/src/VNComms.cpp index 39afdb3..69f335b 100644 --- a/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_comms/src/VNComms.cpp @@ -17,8 +17,7 @@ namespace comms { - bool VNDriver::init() - { + bool VNDriver::init() { // Try to establish a connection to the driver spdlog::info("Opening vn driver."); auto device_name = get_parameter_value("device_name"); @@ -32,8 +31,7 @@ namespace comms auto ec_ret = _serial.open(device_name.value(), ec); - if (ec) - { + if (ec) { spdlog::warn("Error: {}", ec.message()); spdlog::info("failed to open vectornav serial port"); return false; @@ -57,27 +55,23 @@ namespace comms VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr state_estimator, boost::asio::io_context& io, bool &init_not_successful) : core::common::Configurable(json_file_handler, "VNDriver"), _state_estimator(state_estimator), - _serial(io) - { + _serial(io) { init_not_successful = !init(); // Starts read - if(!init_not_successful) - { + if(!init_not_successful) { spdlog::info("Starting vn driver recieve."); _start_recieve(); } } - void VNDriver::log_proto_message(std::shared_ptr msg) - { + void VNDriver::log_proto_message(std::shared_ptr msg) { _state_estimator->handle_recv_process(static_cast>(msg)); this->log(msg); } - void VNDriver::_configure_binary_outputs() - { + void VNDriver::_configure_binary_outputs() { auto num_of_bytes = Packet::genWriteBinaryOutput1( ErrorDetectionMode::ERRORDETECTIONMODE_NONE, @@ -95,24 +89,18 @@ namespace comms boost::asio::async_write(_serial, boost::asio::buffer(_output_buff.data(), num_of_bytes), - [](const boost::system::error_code &ec, std::size_t bytes_transferred) - { - if (!ec) - { + [](const boost::system::error_code &ec, std::size_t bytes_transferred) { + if (!ec) { spdlog::warn("Successfully sent {} bytes.", bytes_transferred); - } - else - { + } else { spdlog::error("Error sending data: {}", ec.message()); } }); } - void VNDriver::_handle_recieve(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts) - { + void VNDriver::_handle_recieve(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts) { auto this_instance = (VNDriver *)userData; - if (packet.type() == vn::protocol::uart::Packet::TYPE_BINARY) - { + if (packet.type() == vn::protocol::uart::Packet::TYPE_BINARY) { vn::math::vec3f vel; // See if this is a binary packet type we are expecting. if (!packet.isCompatible((CommonGroup::COMMONGROUP_YAWPITCHROLL | CommonGroup::COMMONGROUP_ANGULARRATE), // Note use of binary OR to configure flags. @@ -121,8 +109,7 @@ namespace comms GpsGroup::GPSGROUP_NONE, AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY, (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY | InsGroup::INSGROUP_VELU), - GpsGroup::GPSGROUP_NONE)) - { + GpsGroup::GPSGROUP_NONE)) { spdlog::warn("ERROR: packet is not what we want"); return; } @@ -181,23 +168,18 @@ namespace comms vn_ins_msg->set_ins_vel_u(vel_uncertainty); this_instance->log_proto_message(static_cast>(msg_out)); - } - else - { + } else { spdlog::warn("Packet not correct"); } } - void VNDriver::_start_recieve() - { + void VNDriver::_start_recieve() { _serial.async_read_some( boost::asio::buffer(_input_buff), [&](const boost::system::error_code &ec, std::size_t bytesCount) -> void { - if (ec) - { - if (ec != boost::asio::error::operation_aborted) - { + if (ec) { + if (ec != boost::asio::error::operation_aborted) { spdlog::error("ERROR: {}", ec.message()); } return; diff --git a/tests/test_vn.cpp b/tests/test_vn.cpp index d4b2ca8..debe1bf 100644 --- a/tests/test_vn.cpp +++ b/tests/test_vn.cpp @@ -19,10 +19,8 @@ using namespace vn::protocol::uart; std::chrono::time_point init = {}; -void validPacketFoundHandler(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts) -{ - if (packet.type() == vn::protocol::uart::Packet::TYPE_BINARY) - { +void validPacketFoundHandler(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts) { + if (packet.type() == vn::protocol::uart::Packet::TYPE_BINARY) { vn::math::vec3f vel; // See if this is a binary packet type we are expecting. if (!packet.isCompatible((CommonGroup::COMMONGROUP_YAWPITCHROLL | CommonGroup::COMMONGROUP_ANGULARRATE), // Note use of binary OR to configure flags. @@ -31,8 +29,7 @@ void validPacketFoundHandler(void *userData, vn::protocol::uart::Packet &packet, GpsGroup::GPSGROUP_NONE, AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY, (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY), - GpsGroup::GPSGROUP_NONE)) - { + GpsGroup::GPSGROUP_NONE)) { // Not the type of binary packet we are expecting. std::cout << "ERROR: packet is not what we want" << std::endl; return; @@ -66,23 +63,17 @@ void validPacketFoundHandler(void *userData, vn::protocol::uart::Packet &packet, std::cout << "dt ms: " << millis << std::endl; init = curr; - } - else - { + } else { std::cout << "packet not correct" << std::endl; } } -void readData(SerialPort &m_serial, boost::array &m_inputBuf, vn::protocol::uart::PacketFinder &processor) -{ +void readData(SerialPort &m_serial, boost::array &m_inputBuf, vn::protocol::uart::PacketFinder &processor) { m_serial.async_read_some( boost::asio::buffer(m_inputBuf), - [&](const boost::system::error_code &ec, std::size_t bytesCount) - { - if (ec) - { - if (ec != boost::asio::error::operation_aborted) - { + [&](const boost::system::error_code &ec, std::size_t bytesCount) { + if (ec) { + if (ec != boost::asio::error::operation_aborted) { std::cerr << "ERROR: " << ec.message() << std::endl; } return; @@ -94,8 +85,7 @@ void readData(SerialPort &m_serial, boost::array &m_inputBuf, }); } -void write_data(SerialPort &m_serial, boost::array &m_outputBuf) -{ +void write_data(SerialPort &m_serial, boost::array &m_outputBuf) { boost::array m_inputBuf; // Generate binary output data to write @@ -114,32 +104,25 @@ void write_data(SerialPort &m_serial, boost::array &m_outputB GpsGroup::GPSGROUP_NONE); // Perform another blocking write - try - { + try { std::size_t bytes_written = boost::asio::write(m_serial, boost::asio::buffer(m_outputBuf.data(), numOfBytes)); std::cout << "Successfully sent " << bytes_written << " bytes.\n"; - } - catch (const boost::system::system_error &e) - { + } catch (const boost::system::system_error &e) { std::cerr << "Error sending data: " << e.what() << "\n"; } // Perform another blocking read - try - { + try { std::size_t bytes_read = m_serial.read_some(boost::asio::buffer(m_inputBuf)); // Process the received data // processor.processReceivedData((char *)(m_inputBuf.data()), bytes_read); std::cout <<"read"< Date: Thu, 15 Jan 2026 22:12:16 -0500 Subject: [PATCH 3/8] Renamed StateEstimator to StateTracker --- drivebrain_comms/include/VNComms.hpp | 6 +++--- drivebrain_comms/src/VNComms.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/drivebrain_comms/include/VNComms.hpp b/drivebrain_comms/include/VNComms.hpp index 547bdef..12779e8 100644 --- a/drivebrain_comms/include/VNComms.hpp +++ b/drivebrain_comms/include/VNComms.hpp @@ -2,7 +2,7 @@ #include #include -#include +#include // protobuf #include @@ -41,7 +41,7 @@ namespace comms { public core::common::Configurable { public: - VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr state_estimator, boost::asio::io_context &io_context, bool &init_successful); + VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr state_tracker, boost::asio::io_context &io_context, bool &init_successful); ~VNDriver() { spdlog::info("destructed %s", this->get_name()); } @@ -52,7 +52,7 @@ namespace comms { }; private: - std::shared_ptr _state_estimator; + std::shared_ptr _state_tracker; vn::protocol::uart::PacketFinder _processor; diff --git a/drivebrain_comms/src/VNComms.cpp b/drivebrain_comms/src/VNComms.cpp index 69f335b..e625822 100644 --- a/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_comms/src/VNComms.cpp @@ -52,9 +52,9 @@ namespace comms return true; } - VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr state_estimator, boost::asio::io_context& io, bool &init_not_successful) + VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr state_tracker, boost::asio::io_context& io, bool &init_not_successful) : core::common::Configurable(json_file_handler, "VNDriver"), - _state_estimator(state_estimator), + _state_tracker(state_tracker), _serial(io) { init_not_successful = !init(); @@ -67,7 +67,7 @@ namespace comms } void VNDriver::log_proto_message(std::shared_ptr msg) { - _state_estimator->handle_recv_process(static_cast>(msg)); + _state_tracker->handle_recv_process(static_cast>(msg)); this->log(msg); } From 7f8317d77bb7c8ef679d7ece0111ffeba282480c Mon Sep 17 00:00:00 2001 From: Krish Kittur Date: Sun, 18 Jan 2026 02:50:18 -0500 Subject: [PATCH 4/8] include drivebrain comms in root build --- CMakeLists.txt | 2 +- drivebrain_comms/CMakeLists.txt | 3 --- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1c8bf5d..ff38333 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ list(APPEND CMAKE_PREFIX_PATH "${CMAKE_SOURCE_DIR}/cmake") # Custom libs ################################# add_subdirectory(drivebrain_core) -# add_subdirectory(drivebrain_comms) +add_subdirectory(drivebrain_comms) ################################# # Find packages diff --git a/drivebrain_comms/CMakeLists.txt b/drivebrain_comms/CMakeLists.txt index 0abd307..a1cdbe6 100644 --- a/drivebrain_comms/CMakeLists.txt +++ b/drivebrain_comms/CMakeLists.txt @@ -8,9 +8,6 @@ find_package(Protobuf REQUIRED CONFIG) add_library(drivebrain_comms src/CANComms.cpp - src/DBServiceImpl.cpp - src/ETHSendComms.cpp - src/foxglove_server.cpp src/VNComms.cpp ) From 0059ea21271ce2e016d7a1886c8ebdd8993db147 Mon Sep 17 00:00:00 2001 From: akemiyu <43460886+akemiyu@users.noreply.github.com> Date: Sun, 18 Jan 2026 12:48:34 -0500 Subject: [PATCH 5/8] Deleted Configurable & Loggable dependencies --- drivebrain_comms/include/VNComms.hpp | 11 ++++------- drivebrain_comms/src/VNComms.cpp | 21 +++++++++------------ 2 files changed, 13 insertions(+), 19 deletions(-) diff --git a/drivebrain_comms/include/VNComms.hpp b/drivebrain_comms/include/VNComms.hpp index 12779e8..411944e 100644 --- a/drivebrain_comms/include/VNComms.hpp +++ b/drivebrain_comms/include/VNComms.hpp @@ -1,7 +1,5 @@ #pragma once -#include -#include #include // protobuf @@ -30,20 +28,19 @@ #include "libvncxx/vntime.h" #include "libvncxx/packetfinder.h" #include "libvncxx/packet.h" +#include using namespace vn::xplat; using namespace vn::protocol::uart; using SerialPort = boost::asio::serial_port; -using loggertype = core::MsgLogger>; namespace comms { - class VNDriver : public core::common::Loggable>, - public core::common::Configurable + class VNDriver { public: - VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr state_tracker, boost::asio::io_context &io_context, bool &init_successful); + VNDriver(std::shared_ptr state_tracker, boost::asio::io_context &io_context, bool &init_successful); ~VNDriver() { - spdlog::info("destructed %s", this->get_name()); + spdlog::info("destructed %s"); } bool init(); struct config { diff --git a/drivebrain_comms/src/VNComms.cpp b/drivebrain_comms/src/VNComms.cpp index e625822..6540959 100644 --- a/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_comms/src/VNComms.cpp @@ -20,16 +20,16 @@ namespace comms bool VNDriver::init() { // Try to establish a connection to the driver spdlog::info("Opening vn driver."); - auto device_name = get_parameter_value("device_name"); - _config.baud_rate = get_parameter_value("baud_rate").value(); - _config.freq_divisor = get_parameter_value("freq_divisor").value(); - auto port = get_parameter_value("port"); + // auto device_name = get_parameter_value("device_name"); + // _config.baud_rate = get_parameter_value("baud_rate").value(); + // _config.freq_divisor = get_parameter_value("freq_divisor").value(); + // auto port = get_parameter_value("port"); _processor.registerPossiblePacketFoundHandler(this, &VNDriver::_handle_recieve); boost::system::error_code ec; - auto ec_ret = _serial.open(device_name.value(), ec); + // auto ec_ret = _serial.open(device_name.value(), ec); if (ec) { spdlog::warn("Error: {}", ec.message()); @@ -38,7 +38,7 @@ namespace comms } // Set the baud rate of the device along with other configs - _serial.set_option(SerialPort::baud_rate(_config.baud_rate)); + // _serial.set_option(SerialPort::baud_rate(_config.baud_rate)); _serial.set_option(SerialPort::character_size(8)); _serial.set_option(SerialPort::parity(SerialPort::parity::none)); _serial.set_option(SerialPort::stop_bits(SerialPort::stop_bits::one)); @@ -48,13 +48,11 @@ namespace comms spdlog::info("Configuring binary outputs."); _configure_binary_outputs(); - set_configured(); return true; } - VNDriver::VNDriver(core::JsonFileHandler &json_file_handler, std::shared_ptr state_tracker, boost::asio::io_context& io, bool &init_not_successful) - : core::common::Configurable(json_file_handler, "VNDriver"), - _state_tracker(state_tracker), + VNDriver::VNDriver(std::shared_ptr state_tracker, boost::asio::io_context& io, bool &init_not_successful) + : _state_tracker(state_tracker), _serial(io) { init_not_successful = !init(); @@ -67,8 +65,7 @@ namespace comms } void VNDriver::log_proto_message(std::shared_ptr msg) { - _state_tracker->handle_recv_process(static_cast>(msg)); - this->log(msg); + _state_tracker->handle_receive_protobuf_message(static_cast>(msg)); } void VNDriver::_configure_binary_outputs() { From c22ee77e48d822e71d33905c5a8102f29f91aedc Mon Sep 17 00:00:00 2001 From: Kevin Luo Date: Sun, 1 Mar 2026 19:11:41 -0500 Subject: [PATCH 6/8] Begin implementation of initial heading in Mode 0 to expedite startup to Mode 3 via declination offset of magnetic heading --- drivebrain_comms/include/VNComms.hpp | 5 +- drivebrain_comms/src/VNComms.cpp | 121 ++++++++++++++++++++++----- 2 files changed, 104 insertions(+), 22 deletions(-) diff --git a/drivebrain_comms/include/VNComms.hpp b/drivebrain_comms/include/VNComms.hpp index 411944e..15848c7 100644 --- a/drivebrain_comms/include/VNComms.hpp +++ b/drivebrain_comms/include/VNComms.hpp @@ -57,6 +57,8 @@ namespace comms { boost::array _input_buff; SerialPort _serial; config _config; + bool _initial_heading_set = false; + float _local_declination; public: // Public methods @@ -67,6 +69,7 @@ namespace comms { static void _handle_recieve(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts); void _configure_binary_outputs(); void _start_recieve(); - void _set_initial_heading(); + void _set_initial_heading(float initial_heading); + void _try_initialize_heading(float mag_heading, uint8_t ins_mode); }; } \ No newline at end of file diff --git a/drivebrain_comms/src/VNComms.cpp b/drivebrain_comms/src/VNComms.cpp index 6540959..5aa69ea 100644 --- a/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_comms/src/VNComms.cpp @@ -47,7 +47,32 @@ namespace comms // Configures the binary outputs for the device spdlog::info("Configuring binary outputs."); _configure_binary_outputs(); + + // TODO: Separate function for this?? Consider if VN-300 is ARHS-enabled by default?? + // Creating buffer for writing to INS Basic Configuration (Register 67) + auto num_of_bytes = Packet::genWriteInsBasicConfiguration( + ErrorDetectionMode::ERRORDETECTIONMODE_NONE, + (char *)_output_buff.data(), + _output_buff.size(), + 0, // Scenario: AHRS (Attitude Only - no GNSS/INS) + 1, // Set AhrsAiding: provides the ability to switch to using the magnetometer to stabilize heading during times when GNSS-based heading is unavailable. + 0 // Estimate Baseline: if enabled, the sensor will auto-populate the GNSS Compass Estimated Baseline register (Register 97) with the INS-estimated baseline + ); + + // Write the buffer to the VN-300 + boost::asio::async_write(_serial, + boost::asio::buffer(_output_buff.data(), num_of_bytes), + [](const boost::system::error_code &ec, std::size_t bytes_transferred) { + if (!ec) { + spdlog::warn("Successfully sent {} bytes.", bytes_transferred); + } else { + spdlog::error("Error sending data: {}", ec.message()); + } + }); + + // TODO: SET WNN HERE + return true; } @@ -68,8 +93,56 @@ namespace comms _state_tracker->handle_receive_protobuf_message(static_cast>(msg)); } - void VNDriver::_configure_binary_outputs() { + // TODO: FINISH IMPLEMENTATION + // Sets the heading estimate to the angle provided by the user, immediately initializing the INS filter and expediting the startup process + // Must be before INS mode 3 + // Have to feed initial heading within 5 degrees of True North + // Calculate declination (w/Reg21 & 83) and offset Magnetic North to get True North + // Write result to Reg 161 while VN-300 in Mode 0 to set initial heading and expediate startup to Mode 3 + // New method to try and get initial heading from the VN-300 + void VNDriver::_try_initialize_heading(float mag_heading, uint8_t ins_mode) { + if (_initial_heading_set) return; + + if (ins_mode == 0) { + float true_heading = mag_heading + _local_declination; + + _set_initial_heading(true_heading); + + _initial_heading_set = true; + spdlog::info("Applied declination of {} deg for True Heading of {}", _local_declination, true_heading); + } + + return; +} + + // Sets the heading estimate to the angle provided by the user, immediately initializing the INS filter and expediting the startup process + void VNDriver::_set_initial_heading(float initial_heading) { + // Packet namespace does not have set intial heading command. Generate the $VNSIH command. + // The manual shows the format: $VNSIH,heading + #if VN_HAVE_SECURE_CRT + size_t length = sprintf_s((char*)_output_buff.data(), _output_buff.size(), "$VNSIH,%.3f", initial_heading); + #else + size_t length = sprintf((char*)_output_buff.data(), "$VNSIH,%.3f", initial_heading); + #endif + + // Create packet Checksum + auto num_of_bytes = Packet::finalizeCommand(ErrorDetectionMode::ERRORDETECTIONMODE_NONE, (char*)_output_buff.data(), length); + + // Send command to VN-300 + boost::asio::async_write(_serial, + boost::asio::buffer(_output_buff.data(), num_of_bytes), + [](const boost::system::error_code &ec, std::size_t bytes_transferred) { + if (!ec) { + spdlog::warn("Successfully sent {} bytes.", bytes_transferred); + } else { + spdlog::error("Error sending data: {}", ec.message()); + } + }); + } + + void VNDriver::_configure_binary_outputs() { + auto num_of_bytes = Packet::genWriteBinaryOutput1( ErrorDetectionMode::ERRORDETECTIONMODE_NONE, (char *)_output_buff.data(), @@ -83,16 +156,16 @@ namespace comms AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY, (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY | InsGroup::INSGROUP_VELU), GpsGroup::GPSGROUP_NONE); - - boost::asio::async_write(_serial, - boost::asio::buffer(_output_buff.data(), num_of_bytes), - [](const boost::system::error_code &ec, std::size_t bytes_transferred) { - if (!ec) { - spdlog::warn("Successfully sent {} bytes.", bytes_transferred); - } else { - spdlog::error("Error sending data: {}", ec.message()); - } - }); + + boost::asio::async_write(_serial, + boost::asio::buffer(_output_buff.data(), num_of_bytes), + [](const boost::system::error_code &ec, std::size_t bytes_transferred) { + if (!ec) { + spdlog::warn("Successfully sent {} bytes.", bytes_transferred); + } else { + spdlog::error("Error sending data: {}", ec.message()); + } + }); } void VNDriver::_handle_recieve(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts) { @@ -101,16 +174,16 @@ namespace comms vn::math::vec3f vel; // See if this is a binary packet type we are expecting. if (!packet.isCompatible((CommonGroup::COMMONGROUP_YAWPITCHROLL | CommonGroup::COMMONGROUP_ANGULARRATE), // Note use of binary OR to configure flags. - TimeGroup::TIMEGROUP_NONE, - ImuGroup::IMUGROUP_UNCOMPACCEL, - GpsGroup::GPSGROUP_NONE, - AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY, - (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY | InsGroup::INSGROUP_VELU), - GpsGroup::GPSGROUP_NONE)) { + TimeGroup::TIMEGROUP_NONE, + ImuGroup::IMUGROUP_UNCOMPACCEL, + GpsGroup::GPSGROUP_NONE, + AttitudeGroup::ATTITUDEGROUP_LINEARACCELBODY, + (InsGroup::INSGROUP_INSSTATUS | InsGroup::INSGROUP_POSLLA | InsGroup::INSGROUP_VELBODY | InsGroup::INSGROUP_VELU), + GpsGroup::GPSGROUP_NONE)) { spdlog::warn("ERROR: packet is not what we want"); return; } - + // Extract data in correct order auto ypr_data = packet.extractVec3f(); auto angular_rate_data = packet.extractVec3f(); @@ -120,15 +193,20 @@ namespace comms auto pos_lla = packet.extractVec3d(); auto vel_body = packet.extractVec3f(); auto vel_uncertainty = packet.extractFloat(); + + auto ins_mode = static_cast(ins_status & 0b11); // Extract INS mode from status + + // TODO: Attempt to set initial heading + // _try_initialize_heading(ypr_data.x, ins_mode); // Create the protobuf message to send std::shared_ptr msg_out = std::make_shared(); - + hytech_msgs::xyz_vector *linear_vel_msg = msg_out->mutable_vn_vel_m_s(); linear_vel_msg->set_x(vel_body.x); linear_vel_msg->set_y(vel_body.y); linear_vel_msg->set_z(vel_body.z); - + hytech_msgs::xyz_vector *linear_accel_msg = msg_out->mutable_vn_linear_accel_m_ss(); linear_accel_msg->set_x(linear_accel_body.x); linear_accel_msg->set_y(linear_accel_body.y); @@ -154,7 +232,7 @@ namespace comms vn_gps_msg->set_lon(pos_lla.y); hytech_msgs::vn_status *vn_ins_msg = msg_out->mutable_status(); - vn_ins_msg->set_ins_mode(static_cast(ins_status & 0b11)); + vn_ins_msg->set_ins_mode(ins_mode); vn_ins_msg->set_gnss_fix((ins_status >> 2) & 0b1); vn_ins_msg->set_error_imu((ins_status >> 4) & 1); vn_ins_msg->set_error_mag_pres((ins_status >> 5) & 0b1); @@ -187,4 +265,5 @@ namespace comms _start_recieve(); }); } + } From 888cc530fc901f5587d3f699f267b6ca957b8dd5 Mon Sep 17 00:00:00 2001 From: nebudev14 Date: Sat, 4 Apr 2026 17:51:54 -0400 Subject: [PATCH 7/8] minor fixes --- config/config.json | 2 +- drivebrain_comms/include/VNComms.hpp | 7 ++- drivebrain_comms/src/VNComms.cpp | 92 ++++++++++++++++------------ 3 files changed, 57 insertions(+), 44 deletions(-) diff --git a/config/config.json b/config/config.json index eda90a8..cb39f25 100644 --- a/config/config.json +++ b/config/config.json @@ -35,7 +35,7 @@ "dt_rate_hz": 1000, "apply_vectoring_in_regen": true }, - "VNDriver": { + "vn_driver": { "device_name": "/dev/ttyUSB1", "baud_rate": 921600, "port": 1, diff --git a/drivebrain_comms/include/VNComms.hpp b/drivebrain_comms/include/VNComms.hpp index 15848c7..2cdbbc2 100644 --- a/drivebrain_comms/include/VNComms.hpp +++ b/drivebrain_comms/include/VNComms.hpp @@ -3,6 +3,7 @@ #include // protobuf +#include "FoxgloveServer.hpp" #include #include #include @@ -50,8 +51,6 @@ namespace comms { private: std::shared_ptr _state_tracker; - - vn::protocol::uart::PacketFinder _processor; boost::array _output_buff; boost::array _input_buff; @@ -67,9 +66,11 @@ namespace comms { private: // Private methods static void _handle_recieve(void *userData, vn::protocol::uart::Packet &packet, size_t runningIndexOfPacketStart, TimeStamp ts); + void _configure_INS(); void _configure_binary_outputs(); void _start_recieve(); void _set_initial_heading(float initial_heading); void _try_initialize_heading(float mag_heading, uint8_t ins_mode); + }; -} \ No newline at end of file +} diff --git a/drivebrain_comms/src/VNComms.cpp b/drivebrain_comms/src/VNComms.cpp index 5aa69ea..07f179d 100644 --- a/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_comms/src/VNComms.cpp @@ -9,27 +9,32 @@ #include "hytech_msgs.pb.h" #include "base_msgs.pb.h" +#include "libvncxx/types.h" #include "libvncxx/vntime.h" #include "libvncxx/packetfinder.h" #include "libvncxx/packet.h" #include +#include "FoxgloveServer.hpp" +#include "Telemetry.hpp" + namespace comms { bool VNDriver::init() { // Try to establish a connection to the driver spdlog::info("Opening vn driver."); - // auto device_name = get_parameter_value("device_name"); - // _config.baud_rate = get_parameter_value("baud_rate").value(); - // _config.freq_divisor = get_parameter_value("freq_divisor").value(); - // auto port = get_parameter_value("port"); + auto& foxglove = core::FoxgloveServer::instance(); + + auto device_name = foxglove.get_param("vn_driver/device_name").value_or("/dev/ttyUSB1"); + _config.baud_rate = foxglove.get_param("vn_driver/baud_rate").value_or(921600); + _config.freq_divisor = foxglove.get_param("vn_driver/freq_divisor").value_or(1); + auto port = foxglove.get_param("vn_driver/port").value_or(1); _processor.registerPossiblePacketFoundHandler(this, &VNDriver::_handle_recieve); boost::system::error_code ec; - - // auto ec_ret = _serial.open(device_name.value(), ec); + auto ec_ret = _serial.open(device_name, ec); if (ec) { spdlog::warn("Error: {}", ec.message()); @@ -38,7 +43,7 @@ namespace comms } // Set the baud rate of the device along with other configs - // _serial.set_option(SerialPort::baud_rate(_config.baud_rate)); + _serial.set_option(SerialPort::baud_rate(_config.baud_rate)); _serial.set_option(SerialPort::character_size(8)); _serial.set_option(SerialPort::parity(SerialPort::parity::none)); _serial.set_option(SerialPort::stop_bits(SerialPort::stop_bits::one)); @@ -47,31 +52,10 @@ namespace comms // Configures the binary outputs for the device spdlog::info("Configuring binary outputs."); _configure_binary_outputs(); - - // TODO: Separate function for this?? Consider if VN-300 is ARHS-enabled by default?? - // Creating buffer for writing to INS Basic Configuration (Register 67) - auto num_of_bytes = Packet::genWriteInsBasicConfiguration( - ErrorDetectionMode::ERRORDETECTIONMODE_NONE, - (char *)_output_buff.data(), - _output_buff.size(), - 0, // Scenario: AHRS (Attitude Only - no GNSS/INS) - 1, // Set AhrsAiding: provides the ability to switch to using the magnetometer to stabilize heading during times when GNSS-based heading is unavailable. - 0 // Estimate Baseline: if enabled, the sensor will auto-populate the GNSS Compass Estimated Baseline register (Register 97) with the INS-estimated baseline - ); - // Write the buffer to the VN-300 - boost::asio::async_write(_serial, - boost::asio::buffer(_output_buff.data(), num_of_bytes), - [](const boost::system::error_code &ec, std::size_t bytes_transferred) { - if (!ec) { - spdlog::warn("Successfully sent {} bytes.", bytes_transferred); - } else { - spdlog::error("Error sending data: {}", ec.message()); - } - }); - - - // TODO: SET WNN HERE + spdlog::info("INS config"); + _configure_INS(); + return true; } @@ -91,6 +75,7 @@ namespace comms void VNDriver::log_proto_message(std::shared_ptr msg) { _state_tracker->handle_receive_protobuf_message(static_cast>(msg)); + core::log(static_cast>(msg)); } @@ -102,20 +87,19 @@ namespace comms // Write result to Reg 161 while VN-300 in Mode 0 to set initial heading and expediate startup to Mode 3 // New method to try and get initial heading from the VN-300 void VNDriver::_try_initialize_heading(float mag_heading, uint8_t ins_mode) { - if (_initial_heading_set) return; + if (_initial_heading_set) return; - if (ins_mode == 0) { - float true_heading = mag_heading + _local_declination; + if (ins_mode == 0) { + float true_heading = mag_heading + _local_declination; - _set_initial_heading(true_heading); + _set_initial_heading(true_heading); - _initial_heading_set = true; - spdlog::info("Applied declination of {} deg for True Heading of {}", _local_declination, true_heading); + _initial_heading_set = true; + spdlog::info("Applied declination of {} deg for True Heading of {}", _local_declination, true_heading); + } + return; } - return; -} - // Sets the heading estimate to the angle provided by the user, immediately initializing the INS filter and expediting the startup process void VNDriver::_set_initial_heading(float initial_heading) { // Packet namespace does not have set intial heading command. Generate the $VNSIH command. @@ -141,6 +125,34 @@ namespace comms }); } + + void VNDriver::_configure_INS() { + + // TODO: Separate function for this?? Consider if VN-300 is ARHS-enabled by default?? + // Creating buffer for writing to INS Basic Configuration (Register 67) + auto num_of_bytes = Packet::genWriteInsBasicConfiguration( + ErrorDetectionMode::ERRORDETECTIONMODE_NONE, + (char *)_output_buff.data(), + _output_buff.size(), + 0, // Scenario: AHRS (Attitude Only - no GNSS/INS) + 1, // Set AhrsAiding: provides the ability to switch to using the magnetometer to stabilize heading during times when GNSS-based heading is unavailable. + 0 // Estimate Baseline: if enabled, the sensor will auto-populate the GNSS Compass Estimated Baseline register (Register 97) with the INS-estimated baseline + ); + + // Write the buffer to the VN-300 + boost::asio::async_write(_serial, + boost::asio::buffer(_output_buff.data(), num_of_bytes), + [](const boost::system::error_code &ec, std::size_t bytes_transferred) { + if (!ec) { + spdlog::warn("Successfully sent {} bytes.", bytes_transferred); + } else { + spdlog::error("Error sending data: {}", ec.message()); + } + }); + + // TODO: SET WNN HERE + } + void VNDriver::_configure_binary_outputs() { auto num_of_bytes = Packet::genWriteBinaryOutput1( From bc1c17598ee03418d9a87f824b5b1ed31925c98b Mon Sep 17 00:00:00 2001 From: nebudev14 Date: Sat, 4 Apr 2026 19:07:52 -0400 Subject: [PATCH 8/8] update param fetching --- drivebrain_comms/src/VNComms.cpp | 21 +++-- tests/scripts/run_ethernet_driver.cpp | 108 ++++++++++++------------- tests/scripts/run_mcap_logger.cpp | 110 +++++++++++++------------- 3 files changed, 124 insertions(+), 115 deletions(-) diff --git a/drivebrain_comms/src/VNComms.cpp b/drivebrain_comms/src/VNComms.cpp index 07f179d..0f7e6d9 100644 --- a/drivebrain_comms/src/VNComms.cpp +++ b/drivebrain_comms/src/VNComms.cpp @@ -26,15 +26,23 @@ namespace comms spdlog::info("Opening vn driver."); auto& foxglove = core::FoxgloveServer::instance(); - auto device_name = foxglove.get_param("vn_driver/device_name").value_or("/dev/ttyUSB1"); - _config.baud_rate = foxglove.get_param("vn_driver/baud_rate").value_or(921600); - _config.freq_divisor = foxglove.get_param("vn_driver/freq_divisor").value_or(1); - auto port = foxglove.get_param("vn_driver/port").value_or(1); + auto device_name = foxglove.get_param("vn_driver/device_name"); + auto baud_rate = foxglove.get_param("vn_driver/baud_rate"); + auto freq_divisor = foxglove.get_param("vn_driver/freq_divisor"); + auto port = foxglove.get_param("vn_driver/port"); + + if(!(device_name && baud_rate && freq_divisor && port)) { + spdlog::error("Couldn't load all params for VN Driver"); + return false; + } + + _config.baud_rate = baud_rate.value(); + _config.freq_divisor = freq_divisor.value(); _processor.registerPossiblePacketFoundHandler(this, &VNDriver::_handle_recieve); boost::system::error_code ec; - auto ec_ret = _serial.open(device_name, ec); + auto ec_ret = _serial.open(device_name.value(), ec); if (ec) { spdlog::warn("Error: {}", ec.message()); @@ -54,7 +62,8 @@ namespace comms _configure_binary_outputs(); spdlog::info("INS config"); - _configure_INS(); + // TODO: uncomment + // _configure_INS(); return true; diff --git a/tests/scripts/run_ethernet_driver.cpp b/tests/scripts/run_ethernet_driver.cpp index 99cf55e..aaab549 100644 --- a/tests/scripts/run_ethernet_driver.cpp +++ b/tests/scripts/run_ethernet_driver.cpp @@ -1,63 +1,63 @@ -/** - * Script for testing ethernet driver functionality. - * Spawns a mock thread sending protobuf messages over "ethernet" using the ETHSend class. -*/ +// /** +// * Script for testing ethernet driver functionality. +// * Spawns a mock thread sending protobuf messages over "ethernet" using the ETHSend class. +// */ -#include "ETHSendComms.hpp" -#include "ETHRecvComms.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +// #include "ETHSendComms.hpp" +// #include "ETHRecvComms.hpp" +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include -std::atomic running = true; -void sig_handler(int signal) { - if(signal == SIGINT) { - std::cout << "halting\n"; - running = false; - } -} +// std::atomic running = true; +// void sig_handler(int signal) { +// if(signal == SIGINT) { +// std::cout << "halting\n"; +// running = false; +// } +// } -int main(int argc, char* argv[]) { - // Singleton Creation - core::FoxgloveServer::create(argv[1]); - core::MCAPLogger::create("recordings/", mcap::McapWriterOptions(""), argv[1]); - core::MCAPLogger::instance().open_new_mcap("test_1.mcap"); - core::MCAPLogger::instance().init_logging(); +// int main(int argc, char* argv[]) { +// // Singleton Creation +// core::FoxgloveServer::create(argv[1]); +// core::MCAPLogger::create("recordings/", mcap::McapWriterOptions(""), argv[1]); +// core::MCAPLogger::instance().open_new_mcap("test_1.mcap"); +// core::MCAPLogger::instance().init_logging(); - std::signal(SIGINT, sig_handler); +// std::signal(SIGINT, sig_handler); - boost::asio::io_context io_context; - comms::ETHSendComms eth_sender{io_context, 2222, "127.0.0.1"}; - comms::ETHRecvComms eth_recver{io_context, 2222}; +// boost::asio::io_context io_context; +// comms::ETHSendComms eth_sender{io_context, 2222, "127.0.0.1"}; +// comms::ETHRecvComms eth_recver{io_context, 2222}; - std::thread io_thread([&]() { - io_context.run(); - }); +// std::thread io_thread([&]() { +// io_context.run(); +// }); - std::thread t1([&]() { - while(running) { - auto msg = std::make_shared(); - msg->set_max_cell_temp_id(6767); - std::this_thread::sleep_for((std::chrono::seconds(1))); - eth_sender.enqueue_msg_send(msg); - } - }); +// std::thread t1([&]() { +// while(running) { +// auto msg = std::make_shared(); +// msg->set_max_cell_temp_id(6767); +// std::this_thread::sleep_for((std::chrono::seconds(1))); +// eth_sender.enqueue_msg_send(msg); +// } +// }); - if(t1.joinable()) t1.join(); - io_context.stop(); - if(io_thread.joinable()) io_thread.join(); +// if(t1.joinable()) t1.join(); +// io_context.stop(); +// if(io_thread.joinable()) io_thread.join(); - core::MCAPLogger::instance().close_active_mcap(); - core::MCAPLogger::destroy(); - core::FoxgloveServer::destroy(); -} +// core::MCAPLogger::instance().close_active_mcap(); +// core::MCAPLogger::destroy(); +// core::FoxgloveServer::destroy(); +// } diff --git a/tests/scripts/run_mcap_logger.cpp b/tests/scripts/run_mcap_logger.cpp index 2e97052..d19bdd9 100644 --- a/tests/scripts/run_mcap_logger.cpp +++ b/tests/scripts/run_mcap_logger.cpp @@ -1,61 +1,61 @@ -/** - * Script for testing MCAP logging functionality. - * Spawns two mock threads sending protobuf messages at different rates and logs to MCAP. -*/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -std::atomic running = true; - -void sig_handler(int signal) { - if(signal == SIGINT) { - spdlog::info("halting"); - running = false; - } -} - -void get_param_task(int wait_time, core::MsgType msg) { - while(running) { - core::MCAPLogger::instance().log_msg(static_cast(msg)); - std::this_thread::sleep_for((std::chrono::milliseconds(wait_time))); - } -} - -int main(int argc, char* argv[]) { - - core::FoxgloveServer::create(argv[1]); - core::MCAPLogger::create("recordings/", mcap::McapWriterOptions(""), argv[1]); - core::MCAPLogger::instance().open_new_mcap("test_1.mcap"); - core::MCAPLogger::instance().init_logging(); - - std::signal(SIGINT, sig_handler); +// /** +// * Script for testing MCAP logging functionality. +// * Spawns two mock threads sending protobuf messages at different rates and logs to MCAP. +// */ + +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// std::atomic running = true; + +// void sig_handler(int signal) { +// if(signal == SIGINT) { +// spdlog::info("halting"); +// running = false; +// } +// } + +// void get_param_task(int wait_time, core::MsgType msg) { +// while(running) { +// core::MCAPLogger::instance().log_msg(static_cast(msg)); +// std::this_thread::sleep_for((std::chrono::milliseconds(wait_time))); +// } +// } + +// int main(int argc, char* argv[]) { + +// core::FoxgloveServer::create(argv[1]); +// core::MCAPLogger::create("recordings/", mcap::McapWriterOptions(""), argv[1]); +// core::MCAPLogger::instance().open_new_mcap("test_1.mcap"); +// core::MCAPLogger::instance().init_logging(); + +// std::signal(SIGINT, sig_handler); - auto vel_msg = std::make_shared(); - vel_msg->set_velocity_x(1000); - vel_msg->set_velocity_y(10000); +// auto vel_msg = std::make_shared(); +// vel_msg->set_velocity_x(1000); +// vel_msg->set_velocity_y(10000); - auto acu_data = std::make_shared(); - acu_data->set_max_cell_temp_id(676767); +// auto acu_data = std::make_shared(); +// acu_data->set_max_cell_temp_id(676767); - std::thread t1(get_param_task, 20, vel_msg); - std::thread t2(get_param_task, 40, acu_data); +// std::thread t1(get_param_task, 20, vel_msg); +// std::thread t2(get_param_task, 40, acu_data); - if(t1.joinable()) t1.join(); - if(t2.joinable()) t2.join(); - core::MCAPLogger::instance().close_active_mcap(); +// if(t1.joinable()) t1.join(); +// if(t2.joinable()) t2.join(); +// core::MCAPLogger::instance().close_active_mcap(); - core::MCAPLogger::destroy(); - core::FoxgloveServer::destroy(); +// core::MCAPLogger::destroy(); +// core::FoxgloveServer::destroy(); -} +// }