From d7cac5adf4e1f5aaa0a73eb20db74c6077b48790 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Thu, 11 Jun 2026 18:21:49 +0200 Subject: [PATCH 1/3] feat: convert thruster interface to can --- auv_setup/config/robots/nautilus.yaml | 2 + motion/thruster_interface_auv/CMakeLists.txt | 3 +- .../config/thruster_interface_auv_config.yaml | 7 +- .../include/can_interface.hpp | 59 +++ .../thruster_interface_auv_driver.hpp | 119 +---- .../thruster_interface_auv_ros.hpp | 6 +- .../src/can_interface.cpp | 199 ++++++++ .../src/thruster_interface_auv_driver.cpp | 446 +++++++----------- .../src/thruster_interface_auv_ros.cpp | 23 +- 9 files changed, 477 insertions(+), 387 deletions(-) create mode 100644 motion/thruster_interface_auv/include/can_interface.hpp create mode 100644 motion/thruster_interface_auv/src/can_interface.cpp diff --git a/auv_setup/config/robots/nautilus.yaml b/auv_setup/config/robots/nautilus.yaml index 2e81fdeb8..7b568a87a 100644 --- a/auv_setup/config/robots/nautilus.yaml +++ b/auv_setup/config/robots/nautilus.yaml @@ -99,6 +99,7 @@ wrench_input: "wrench_input" thruster_forces: "thruster_forces" pwm_output: "pwm" + camera_light: "camera_light" current: "power_sense_module/current" voltage: "power_sense_module/voltage" pressure: "pressure" @@ -131,6 +132,7 @@ landmark_convergence: "landmark_convergence" waypoint_manager: "waypoint_manager" + services: set_operation_mode: "set_operation_mode" set_killswitch: "set_killswitch" diff --git a/motion/thruster_interface_auv/CMakeLists.txt b/motion/thruster_interface_auv/CMakeLists.txt index ef8cad9dc..390d40cb7 100644 --- a/motion/thruster_interface_auv/CMakeLists.txt +++ b/motion/thruster_interface_auv/CMakeLists.txt @@ -20,7 +20,8 @@ set(LIB_NAME "${PROJECT_NAME}_component") add_library(${LIB_NAME} SHARED src/thruster_interface_auv_ros.cpp - src/thruster_interface_auv_driver.cpp) + src/thruster_interface_auv_driver.cpp + src/can_interface.cpp) ament_target_dependencies(${LIB_NAME} PUBLIC rclcpp diff --git a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml index 099bddda3..92ce27a46 100644 --- a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml +++ b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml @@ -19,10 +19,9 @@ # 20V: # LEFT: [2.55792, 27.51178, 147.67001, 1472.23069] # RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] - - uart: - device: "/dev/serial0" - baud_rate: 115200 + + can: + interface: "vcan0" debug: flag: True diff --git a/motion/thruster_interface_auv/include/can_interface.hpp b/motion/thruster_interface_auv/include/can_interface.hpp new file mode 100644 index 000000000..03e71ef03 --- /dev/null +++ b/motion/thruster_interface_auv/include/can_interface.hpp @@ -0,0 +1,59 @@ +#ifndef CAN_INTERFACE_H +#define CAN_INTERFACE_H + +#include +#include +#include +#include +#include +#include + +enum class can_status { + OK = 0, + ERR_ALREADY_INITIALIZED = -1, + ERR_SOCKET = -2, + ERR_CANFD_SUPPORT = -3, + ERR_LOOPBACK = -4, + ERR_INTERFACE_INDEX = -5, + ERR_BIND = -6, + ERR_NOT_INITIALIZED = -7, + ERR_DATA_LENGTH = -8, + ERR_SEND = -9, + ERR_RECEIVE = -10, + ERR_SETTING_TIMEOUT = -11, + ERR_SETTING_FILTER = -12, + ERR_CLEARING_FILTERS = -13 +}; + +class can_interface { + private: + int socket_fd_; + bool is_initialized_; + std::string interface_name_; + + std::thread receive_thread_; + std::atomic receiving_ = false; + + public: + can_interface(); + ~can_interface(); + + can_status init(const std::string& ifname = "can0"); + can_status send(uint32_t can_id, + const uint8_t* data, + uint8_t len, + bool use_brs = true); + can_status receive(struct canfd_frame& frame); + can_status receive(struct canfd_frame& frame, int timeout_ms); + can_status start_async_receive( + std::function callback); + void stop_async_receive(); + can_status set_filter(uint32_t can_id, uint32_t can_mask = 0x7FF); + can_status set_filters(const struct can_filter* filters, + size_t num_filters); + can_status clear_filters(); + bool is_initialized() const; + std::string get_interface_name() const; +}; + +#endif // CAN_INTERFACE_H diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp index 3c9d1347c..9c770cdf2 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_driver.hpp @@ -1,18 +1,17 @@ #ifndef THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ #define THRUSTER_INTERFACE_AUV__THRUSTER_INTERFACE_AUV_DRIVER_HPP_ +#include "can_interface.hpp" + #include -// Need to include utility before asio -// clang-format off -#include -#include -// clang-format on #include +#include #include #include -#include #include +#include + /** * @brief struct to hold the parameters for a single thruster */ @@ -42,7 +41,7 @@ using CurrentMeasurementsCallback = /** * @brief class instantiated by ThrusterInterfaceAUVNode to control the * thrusters, takes the thruster forces and converts them to PWM signals to be - * sent via UART to the ESC controller. + * sent via CAN FD to the ESC controller. * * @details Based on the datasheets found in /resources, approximate the map * with a piecewise (>0 and <0) third order polynomial. @@ -53,8 +52,8 @@ using CurrentMeasurementsCallback = * re-implemented in the future for more flexibility if we ever need it to * operate at different voltages in different situations. * - * @note Over UART, the PWM values are packed into a framed packet: - * [magic][id][length][payload][checksum], where the payload is 8 uint16_t. + * @note Over CAN FD, the PWM values are packed into one CAN FD frame payload, + * where the payload is 8 uint16_t. */ class ThrusterInterfaceAUVDriver { public: @@ -64,10 +63,8 @@ class ThrusterInterfaceAUVDriver { * @brief called from ThrusterInterfaceAUVNode .cpp when instantiating the * object, initializes all the params. * - * @param serial_device serial device used to communicate - * (for example /dev/ttyUSB0) - * @param baud_rate UART baud rate - * @param packet_id packet ID sent in the UART frame + * @param can_interface_name CAN interface used to communicate + * (for example can0 or vcan0) * @param thruster_parameters describe mapping, direction, min and max pwm * value for each thruster * @param right_coeffs RIGHT(>0) third order polynomial @@ -75,22 +72,21 @@ class ThrusterInterfaceAUVDriver { * @param left_coeffs LEFT(<0) third order polynomial coefficients */ ThrusterInterfaceAUVDriver( - const std::string& serial_device, - unsigned int baud_rate, + const std::string& can_interface_name, const std::vector& thruster_parameters, const std::vector& right_coeffs, const std::vector& left_coeffs); /** - * @brief initializes UART + * @brief initializes CAN FD * @return 0 on success, negative number on failure */ - int init_uart(); + int init_can(); /** * @brief calls both 1) interpolate_forces_to_pwm() to * convert the thruster forces to PWM values and 2) send_data_to_escs() to - * send them over UART + * send them over CAN FD * * @param thruster_forces_array vector of forces for each thruster * @@ -113,6 +109,8 @@ class ThrusterInterfaceAUVDriver { void set_killswitch_event_callback(KillswitchEventCallback callback); void set_current_measurements_callback( CurrentMeasurementsCallback callback); + int disable_thrusters(); + int enable_thrusters(); private: /** @@ -148,102 +146,25 @@ class ThrusterInterfaceAUVDriver { /** * @brief only takes the pwm values computed and sends them - * over UART as a framed packet + * over CAN FD * * @param thruster_pwm_array vector of pwm values to send * @return 0 on success, -1 on failure */ int send_data_to_escs(const std::vector& thruster_pwm_array); + void handle_can_frame(const struct canfd_frame& frame, can_status status); - /** - * @brief create UART packet with format: - * [magic][id][length][payload][checksum] - * - * @param id packet ID - * @param thruster_pwm_array vector of 8 pwm values to send as payload - * - * @return std::vector serialized packet bytes - */ - std::vector create_packet( - std::uint8_t id, - const std::vector& thruster_pwm_array) const; - - /** - * @brief convert Newtons to Kg - * - * @param force Newtons - * - * @return double Kg - */ static constexpr double to_kg(double force) { return force / 9.80665; } - /** - * @brief start the asynchronous UART receive loop - */ - void start_receive(); - - /** - * @brief issue one asynchronous read on the UART port - */ - void do_receive(); - - /** - * @brief process the accumulated receive buffer and extract valid frames - */ - void process_receive_buffer(); - - /** - * @brief handle one decoded UART frame - * - * @param frame_bytes complete frame bytes including header and checksum - */ - void handle_received_frame(const std::vector& frame_bytes); - - /** - * @brief compute checksum for framed UART packets - * - * @param msg_id message id - * @param length payload length - * @param payload pointer to payload bytes - * - * @return checksum byte - */ - static std::uint8_t compute_checksum(std::uint8_t msg_id, - std::uint8_t length, - const std::uint8_t* payload); - private: - static constexpr std::uint8_t UART_START_BYTE = 0xAA; - static constexpr std::size_t MAX_PAYLOAD_SIZE = 64; - static constexpr std::size_t READ_CHUNK_SIZE = 256; - - // Outgoing - static constexpr std::uint8_t MSG_TURN_THRUSTERS_OFF = 0x01U; - static constexpr std::uint8_t MSG_TURN_LIGHTS_OFF = 0x02U; - static constexpr std::uint8_t MSG_RESET = 0x03; - static constexpr std::uint8_t MSG_SET_THRUSTER_PWM = 0x04; - static constexpr std::uint8_t MSG_SET_LIGHT_PWM = 0x05; - // Incoming - static constexpr std::uint8_t MSG_FLT_EVENT = 0x10; - static constexpr std::uint8_t MSG_PGOOD_EVENT = 0x11; - static constexpr std::uint8_t MSG_KILLSWITCH_EVENT = 0x12; - static constexpr std::uint8_t MSG_CURRENT_MEASUREMENTS = 0x13; - - std::string serial_device_; - unsigned int baud_rate_; - - asio::io_context io_; - asio::serial_port serial_{io_}; - std::thread io_thread_; + std::string can_interface_name_; + can_interface can_; std::vector thruster_parameters_; std::vector right_coeffs_; std::vector left_coeffs_; std::uint16_t idle_pwm_value_{1500}; - std::array read_buf_{}; - std::vector receive_buffer_; - FaultEventCallback fault_event_callback_; PGoodEventCallback pgood_event_callback_; KillswitchEventCallback killswitch_event_callback_; diff --git a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp index b35098722..5a96d6e76 100644 --- a/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp +++ b/motion/thruster_interface_auv/include/thruster_interface_auv/thruster_interface_auv_ros.hpp @@ -73,8 +73,7 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { void pwm_callback(); private: - std::string serial_device_; - unsigned int baud_rate_; + std::string can_interface_; std::string subscriber_topic_name_; std::string publisher_topic_name_; @@ -88,7 +87,7 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { bool debug_flag_{false}; std::unique_ptr - thruster_driver_; ///<-- UART/USART thruster driver + thruster_driver_; ///<-- CAN FD thruster driver rclcpp::Subscription::SharedPtr thruster_forces_subscriber_; ///<-- thruster forces subscriber @@ -112,6 +111,7 @@ class ThrusterInterfaceAUVNode : public rclcpp::Node { rclcpp::Time last_msg_time_; rclcpp::Duration watchdog_timeout_ = rclcpp::Duration::from_seconds(1.0); bool watchdog_triggered_ = false; + /** * @brief Manages parameter events for the node. * diff --git a/motion/thruster_interface_auv/src/can_interface.cpp b/motion/thruster_interface_auv/src/can_interface.cpp new file mode 100644 index 000000000..5c8ad8a03 --- /dev/null +++ b/motion/thruster_interface_auv/src/can_interface.cpp @@ -0,0 +1,199 @@ +#include "can_interface.hpp" +#include +#include +#include +#include +#include +#include +#include + +can_interface::can_interface() : socket_fd_(-1), is_initialized_(false) {} + +can_interface::~can_interface() { + if (is_initialized_) { + close(socket_fd_); + } +} + +can_status can_interface::init(const std::string& ifname) { + interface_name_ = ifname; + + socket_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (socket_fd_ < 0) { + return can_status::ERR_SOCKET; + } + + int enable_canfd = 1; + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, + sizeof(enable_canfd)) < 0) { + close(socket_fd_); + return can_status::ERR_CANFD_SUPPORT; + } + + struct ifreq ifr; + std::strcpy(ifr.ifr_name, interface_name_.c_str()); + if (ioctl(socket_fd_, SIOCGIFINDEX, &ifr) < 0) { + close(socket_fd_); + return can_status::ERR_INTERFACE_INDEX; + } + + struct sockaddr_can addr; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(socket_fd_, (struct sockaddr*)&addr, sizeof(addr)) < 0) { + close(socket_fd_); + return can_status::ERR_BIND; + } + + is_initialized_ = true; + + return can_status::OK; +} + +can_status can_interface::send(uint32_t can_id, + const uint8_t* data, + uint8_t len, + bool use_brs) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + if (len > 64) { + return can_status::ERR_DATA_LENGTH; + } + + struct canfd_frame frame; + memset(&frame, 0, sizeof(frame)); + + frame.can_id = can_id; + frame.len = len; + frame.flags = use_brs ? CANFD_BRS : 0; + + if (data != nullptr && len > 0) { + memcpy(frame.data, data, len); + } + + if (write(socket_fd_, &frame, sizeof(frame)) != sizeof(frame)) { + return can_status::ERR_SEND; + } + + return can_status::OK; +} + +can_status can_interface::receive(struct canfd_frame& frame) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + ssize_t nbytes = read(socket_fd_, &frame, sizeof(frame)); + + if (nbytes != CANFD_MTU) { + return can_status::ERR_RECEIVE; + } + + return can_status::OK; +} + +can_status can_interface::receive(struct canfd_frame& frame, int timeout_ms) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct timeval tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + if (setsockopt(socket_fd_, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) { + return can_status::ERR_SETTING_TIMEOUT; + } + + ssize_t nbytes = read(socket_fd_, &frame, sizeof(frame)); + + if (nbytes != CANFD_MTU) { + return can_status::ERR_RECEIVE; + } + + return can_status::OK; +} + +can_status can_interface::start_async_receive( + std::function callback) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + receiving_ = true; + + receive_thread_ = std::thread([this, callback]() { + while (receiving_) { + struct canfd_frame frame; + can_status status = receive(frame, 1000); + callback(frame, status); + } + }); + return can_status::OK; +} + +void can_interface::stop_async_receive() { + receiving_ = false; + if (receive_thread_.joinable()) { + receive_thread_.join(); + } +} + +can_status can_interface::set_filter(uint32_t can_id, uint32_t can_mask) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct can_filter filter; + filter.can_id = can_id; + filter.can_mask = can_mask; + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, + sizeof(filter)) < 0) { + return can_status::ERR_SETTING_FILTER; + } + + return can_status::OK; +} + +can_status can_interface::set_filters(const struct can_filter* filters, + size_t num_filters) { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, filters, + num_filters * sizeof(struct can_filter)) < 0) { + return can_status::ERR_SETTING_FILTER; + } + + return can_status::OK; +} + +can_status can_interface::clear_filters() { + if (!is_initialized_) { + return can_status::ERR_NOT_INITIALIZED; + } + + struct can_filter filter; + filter.can_id = 0x0; + filter.can_mask = 0x0; + + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, + sizeof(filter)) < 0) { + return can_status::ERR_CLEARING_FILTERS; + } + + return can_status::OK; +} + +bool can_interface::is_initialized() const { + return is_initialized_; +} + +std::string can_interface::get_interface_name() const { + return interface_name_; +} diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index 1ccd73041..cfdeeda80 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -1,23 +1,35 @@ #include "thruster_interface_auv/thruster_interface_auv_driver.hpp" #include -#include +#include +#include #include #include #include -#include -#include -#include +#include #include +#include + + + +static constexpr std::uint32_t CAN_ID_DISABLE_THRUSTERS = 0x369U; +static constexpr std::uint32_t CAN_ID_ENABLE_THRUSTERS = 0x36AU; +static constexpr std::uint32_t CAN_ID_SET_THRUSTERS_PWM = 0x36BU; +static constexpr std::uint32_t CAN_ID_SET_LIGHT_PWM = 0x36CU; + +static constexpr std::uint32_t CAN_ID_FLT_EVENT = 0x36DU; +static constexpr std::uint32_t CAN_ID_PGOOD_EVENT = 0x36EU; +static constexpr std::uint32_t CAN_ID_KILLSWITCH_EVENT = 0x36FU; +static constexpr std::uint32_t CAN_ID_CURRENT_MEASUREMENTS = 0x370U; + + ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( - const std::string& serial_device, - unsigned int baud_rate, + const std::string& can_interface_name, const std::vector& thruster_parameters, const std::vector& right_coeffs, const std::vector& left_coeffs) - : serial_device_(serial_device), - baud_rate_(baud_rate), + : can_interface_name_(can_interface_name), thruster_parameters_(thruster_parameters), right_coeffs_(right_coeffs), left_coeffs_(left_coeffs) { @@ -26,63 +38,53 @@ ThrusterInterfaceAUVDriver::ThrusterInterfaceAUVDriver( } ThrusterInterfaceAUVDriver::~ThrusterInterfaceAUVDriver() { - std::error_code ec; - - if (serial_.is_open()) { + if (can_.is_initialized()) { send_data_to_escs(std::vector( thruster_parameters_.size(), idle_pwm_value_)); - - serial_.cancel(ec); - serial_.close(ec); - } - - io_.stop(); - - if (io_thread_.joinable()) { - io_thread_.join(); + can_.stop_async_receive(); } } -int ThrusterInterfaceAUVDriver::init_uart() { - std::error_code ec; - - serial_.open(serial_device_, ec); - if (ec) { - return -1; - } - serial_.set_option(asio::serial_port::baud_rate(baud_rate_), ec); - if (ec) { - return -1; - } - - serial_.set_option(asio::serial_port::character_size(8), ec); - if (ec) { - return -1; +int ThrusterInterfaceAUVDriver::init_can() { + can_status status = can_.init(can_interface_name_); + if (status != can_status::OK) { + return static_cast(status); } - serial_.set_option( - asio::serial_port::parity(asio::serial_port::parity::none), ec); - if (ec) { - return -1; + struct can_filter filters[] = { + { + .can_id = CAN_ID_FLT_EVENT, + .can_mask = CAN_SFF_MASK, + }, + { + .can_id = CAN_ID_PGOOD_EVENT, + .can_mask = CAN_SFF_MASK, + }, + { + .can_id = CAN_ID_KILLSWITCH_EVENT, + .can_mask = CAN_SFF_MASK, + }, + { + .can_id = CAN_ID_CURRENT_MEASUREMENTS, + .can_mask = CAN_SFF_MASK, + }, + }; + + status = can_.set_filters(filters, sizeof(filters) / sizeof(filters[0])); + if (status != can_status::OK) { + return static_cast(status); } - serial_.set_option( - asio::serial_port::stop_bits(asio::serial_port::stop_bits::one), ec); - if (ec) { - return -1; - } + status = can_.start_async_receive( + [this](const struct canfd_frame& frame, can_status rx_status) { + handle_can_frame(frame, rx_status); + }); - serial_.set_option( - asio::serial_port::flow_control(asio::serial_port::flow_control::none), - ec); - if (ec) { - return -1; + if (status != can_status::OK) { + return static_cast(status); } - start_receive(); - io_thread_ = std::thread([this]() { io_.run(); }); - return 0; } @@ -116,124 +118,34 @@ std::uint16_t ThrusterInterfaceAUVDriver::force_to_pwm(double force) { std::uint16_t ThrusterInterfaceAUVDriver::calc_poly( double force, const std::vector& coeffs) { - return static_cast(coeffs[0] * std::pow(force, 3) + - coeffs[1] * std::pow(force, 2) + - coeffs[2] * force + coeffs[3]); -} - -std::vector ThrusterInterfaceAUVDriver::create_packet( - std::uint8_t id, - const std::vector& thruster_pwm_array) const { - std::vector packet; - - packet.reserve(1 + 1 + 1 + - thruster_pwm_array.size() * sizeof(std::uint16_t) + 1); - - packet.push_back(UART_START_BYTE); - packet.push_back(id); - - const std::uint8_t length = static_cast( - thruster_pwm_array.size() * sizeof(std::uint16_t)); - packet.push_back(length); - - for (std::uint16_t value : thruster_pwm_array) { - packet.push_back(static_cast(value & 0xFF)); - packet.push_back(static_cast((value >> 8) & 0xFF)); - } - - std::uint8_t checksum = id ^ length; - for (std::uint16_t value : thruster_pwm_array) { - checksum ^= static_cast(value & 0xFF); - checksum ^= static_cast((value >> 8) & 0xFF); - } - - packet.push_back(checksum); - - return packet; -} - -int ThrusterInterfaceAUVDriver::send_data_to_escs( - const std::vector& thruster_pwm_array) { - if (!serial_.is_open()) { - return -1; - } - - const auto packet = create_packet(MSG_SET_THRUSTER_PWM, thruster_pwm_array); - constexpr std::size_t header_size = 3; - - if (packet.size() < header_size) { - return -1; - } - - std::error_code ec; - - const auto header_bytes_written = - asio::write(serial_, asio::buffer(packet.data(), header_size), ec); - - if (ec || header_bytes_written != header_size) { - return -1; - } - - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - - const auto remaining_size = packet.size() - header_size; - const auto payload_bytes_written = asio::write( - serial_, asio::buffer(packet.data() + header_size, remaining_size), ec); - - if (ec || payload_bytes_written != remaining_size) { - return -1; - } - - return 0; -} - -int ThrusterInterfaceAUVDriver::set_camera_light(float percentage) { - if (!serial_.is_open()) { - return -1; - } - - std::vector camera_light_pwm_array(1); - camera_light_pwm_array[0] = - static_cast(1100 + 800 * percentage); - - const auto packet = - create_packet(MSG_SET_LIGHT_PWM, camera_light_pwm_array); - constexpr std::size_t header_size = 3; - - if (packet.size() < header_size) { - return -1; - } - - std::error_code ec; - - const auto header_bytes_written = - asio::write(serial_, asio::buffer(packet.data(), header_size), ec); - - if (ec || header_bytes_written != header_size) { - return -1; - } - - std::this_thread::sleep_for(std::chrono::milliseconds(5)); - - const auto remaining_size = packet.size() - header_size; - const auto payload_bytes_written = asio::write( - serial_, asio::buffer(packet.data() + header_size, remaining_size), ec); - - if (ec || payload_bytes_written != remaining_size) { - return -1; + if (coeffs.size() < 4) { + return idle_pwm_value_; } - return 0; + return static_cast( + coeffs[0] * std::pow(force, 3) + + coeffs[1] * std::pow(force, 2) + + coeffs[2] * force + + coeffs[3]); } std::optional> ThrusterInterfaceAUVDriver::drive_thrusters( const std::vector& thruster_forces_array) { + if (thruster_forces_array.size() < thruster_parameters_.size()) { + return std::nullopt; + } + std::vector mapped_forces(thruster_parameters_.size()); for (std::size_t i = 0; i < thruster_parameters_.size(); ++i) { const auto& param = thruster_parameters_[i]; const std::size_t idx = param.mapping; + + if (idx >= thruster_forces_array.size()) { + return std::nullopt; + } + const double raw_force = thruster_forces_array[idx]; mapped_forces[i] = raw_force * param.direction; } @@ -248,185 +160,185 @@ ThrusterInterfaceAUVDriver::drive_thrusters( return thruster_pwm_array; } -void ThrusterInterfaceAUVDriver::start_receive() { - do_receive(); -} - -void ThrusterInterfaceAUVDriver::do_receive() { - serial_.async_read_some( - asio::buffer(read_buf_), - [this](const std::error_code& ec, std::size_t bytes_transferred) { - if (ec) { - return; - } - - receive_buffer_.insert(receive_buffer_.end(), read_buf_.begin(), - read_buf_.begin() + bytes_transferred); - - process_receive_buffer(); - do_receive(); - }); -} - -std::uint8_t ThrusterInterfaceAUVDriver::compute_checksum( - std::uint8_t msg_id, - std::uint8_t length, - const std::uint8_t* payload) { - std::uint8_t checksum = msg_id ^ length; - for (std::size_t i = 0; i < length; ++i) { - checksum ^= payload[i]; +int ThrusterInterfaceAUVDriver::send_data_to_escs( + const std::vector& thruster_pwm_array) { + if (!can_.is_initialized()) { + return static_cast(can_status::ERR_NOT_INITIALIZED); } - return checksum; -} - -void ThrusterInterfaceAUVDriver::process_receive_buffer() { - while (true) { - if (receive_buffer_.size() < 4) { - return; - } - - auto start_it = std::find(receive_buffer_.begin(), - receive_buffer_.end(), UART_START_BYTE); - - if (start_it == receive_buffer_.end()) { - receive_buffer_.clear(); - return; - } + if (thruster_pwm_array.size() != 8) { + return -1; + } - if (start_it != receive_buffer_.begin()) { - receive_buffer_.erase(receive_buffer_.begin(), start_it); - } + std::array payload{}; - if (receive_buffer_.size() < 4) { - return; - } + for (std::size_t i = 0; i < thruster_pwm_array.size(); ++i) { + const std::uint16_t value = thruster_pwm_array[i]; - const std::uint8_t start = receive_buffer_[0]; - const std::uint8_t msg_id = receive_buffer_[1]; - const std::uint8_t length = receive_buffer_[2]; + payload[2 * i] = + static_cast(value & 0xFF); - if (start != UART_START_BYTE) { - receive_buffer_.erase(receive_buffer_.begin()); - continue; - } + payload[2 * i + 1] = + static_cast((value >> 8) & 0xFF); + } - if (length > MAX_PAYLOAD_SIZE) { - receive_buffer_.erase(receive_buffer_.begin()); - continue; - } + const can_status status = can_.send( + CAN_ID_SET_THRUSTERS_PWM, + payload.data(), + static_cast(payload.size()), + true // use BRS + ); - const std::size_t full_frame_size = - 4u + static_cast(length); + if (status != can_status::OK) { + return static_cast(status); + } - if (receive_buffer_.size() < full_frame_size) { - return; - } + return 0; +} - const std::uint8_t* payload_ptr = receive_buffer_.data() + 3; - const std::uint8_t received_checksum = receive_buffer_[3 + length]; - const std::uint8_t expected_checksum = - compute_checksum(msg_id, length, payload_ptr); +int ThrusterInterfaceAUVDriver::set_camera_light(float percentage) { + if (!can_.is_initialized()) { + return static_cast(can_status::ERR_NOT_INITIALIZED); + } - if (received_checksum != expected_checksum) { - receive_buffer_.erase(receive_buffer_.begin()); - continue; - } + percentage = std::clamp(percentage, 0.0f, 1.0f); - std::vector frame_bytes( - receive_buffer_.begin(), - receive_buffer_.begin() + - static_cast(full_frame_size)); + const std::uint16_t pwm = + static_cast(1100.0f + 800.0f * percentage); - handle_received_frame(frame_bytes); + std::array payload{}; + payload[0] = static_cast(pwm & 0xFF); + payload[1] = static_cast((pwm >> 8) & 0xFF); - receive_buffer_.erase(receive_buffer_.begin(), - receive_buffer_.begin() + - static_cast(full_frame_size)); - } -} + const can_status status = can_.send( + CAN_ID_SET_LIGHT_PWM, + payload.data(), + static_cast(payload.size()), + true + ); -void ThrusterInterfaceAUVDriver::handle_received_frame( - const std::vector& frame_bytes) { - if (frame_bytes.size() < 4) { - return; + if (status != can_status::OK) { + return static_cast(status); } - const std::uint8_t msg_id = frame_bytes[1]; - const std::uint8_t length = frame_bytes[2]; + return 0; +} - if (frame_bytes.size() != static_cast(length) + 4u) { +void ThrusterInterfaceAUVDriver::handle_can_frame( + const struct canfd_frame& frame, + can_status status) { + if (status != can_status::OK) { return; } - const std::uint8_t* payload = frame_bytes.data() + 3; - - switch (msg_id) { - case MSG_FLT_EVENT: { - if (length != 2) { + switch (frame.can_id) { + case CAN_ID_FLT_EVENT: { + if (frame.len != 2) { break; } - const std::uint8_t channel = payload[0]; - const std::uint8_t code = payload[1]; + const std::uint8_t channel = frame.data[0]; + const std::uint8_t code = frame.data[1]; if (fault_event_callback_) { fault_event_callback_(channel, code); } + break; } - case MSG_PGOOD_EVENT: { - if (length != 2) { + case CAN_ID_PGOOD_EVENT: { + if (frame.len != 2) { break; } - const std::uint8_t channel = payload[0]; - const std::uint8_t code = payload[1]; + const std::uint8_t channel = frame.data[0]; + const std::uint8_t code = frame.data[1]; if (pgood_event_callback_) { pgood_event_callback_(channel, code); } + break; } - case MSG_KILLSWITCH_EVENT: { - if (length != 0) { + case CAN_ID_KILLSWITCH_EVENT: { + if (frame.len != 0) { break; } if (killswitch_event_callback_) { killswitch_event_callback_(); } + break; } - case MSG_CURRENT_MEASUREMENTS: { + case CAN_ID_CURRENT_MEASUREMENTS: { constexpr std::size_t num_currents = 8; constexpr std::size_t expected_length = num_currents * sizeof(float); - if (length != expected_length) { + if (frame.len != expected_length) { break; } std::array currents{}; - - std::memcpy(currents.data(), payload, expected_length); + std::memcpy(currents.data(), frame.data, expected_length); if (current_measurements_callback_) { current_measurements_callback_(currents); } + break; } - default: { + default: break; - } } } +int ThrusterInterfaceAUVDriver::disable_thrusters() { + if (!can_.is_initialized()) { + return static_cast(can_status::ERR_NOT_INITIALIZED); + } + + const std::uint8_t dummy = 0; + + const can_status status = can_.send( + CAN_ID_DISABLE_THRUSTERS, + &dummy, + 0, + true); + + if (status != can_status::OK) { + return static_cast(status); + } + + return 0; +} + +int ThrusterInterfaceAUVDriver::enable_thrusters() { + if (!can_.is_initialized()) { + return static_cast(can_status::ERR_NOT_INITIALIZED); + } + + const std::uint8_t dummy = 0; + + const can_status status = can_.send( + CAN_ID_ENABLE_THRUSTERS, + &dummy, + 0, + true); + + if (status != can_status::OK) { + return static_cast(status); + } + + return 0; +} + void ThrusterInterfaceAUVDriver::set_fault_event_callback( FaultEventCallback callback) { fault_event_callback_ = std::move(callback); diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp index 8a95a503c..9b0e8cf82 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_ros.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -63,8 +64,7 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode( vortex::utils::qos_profiles::sensor_data_profile(10)); thruster_driver_ = std::make_unique( - serial_device_, baud_rate_, thruster_parameters_, right_coeffs_, - left_coeffs_); + can_interface_, thruster_parameters_, right_coeffs_, left_coeffs_); thruster_driver_->set_fault_event_callback( [this](std::uint8_t channel, std::uint8_t code) { @@ -93,11 +93,11 @@ ThrusterInterfaceAUVNode::ThrusterInterfaceAUVNode( current_measurements_publisher_->publish(msg); }); - if (thruster_driver_->init_uart() != 0) { - spdlog::error("Failed to initialize UART thruster driver"); + if (thruster_driver_->init_can() != 0) { + spdlog::error("Failed to initialize CAN FD thruster driver"); } else { - spdlog::info("UART thruster driver initialized on {} @ {} baud", - serial_device_, baud_rate_); + spdlog::info("CAN FD thruster driver initialized on {}", + can_interface_); } thruster_forces_array_ = std::vector(8, 0.0); @@ -202,8 +202,7 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { this->declare_parameter>("coeffs.16V.LEFT"); this->declare_parameter>("coeffs.16V.RIGHT"); - this->declare_parameter("uart.device"); - this->declare_parameter("uart.baud_rate"); + this->declare_parameter("can.interface"); this->declare_parameter("topics.thruster_forces"); this->declare_parameter("topics.pwm_output"); @@ -228,9 +227,7 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { left_coeffs_ = this->get_parameter("coeffs.16V.LEFT").as_double_array(); right_coeffs_ = this->get_parameter("coeffs.16V.RIGHT").as_double_array(); - serial_device_ = this->get_parameter("uart.device").as_string(); - baud_rate_ = static_cast( - this->get_parameter("uart.baud_rate").as_int()); + can_interface_ = this->get_parameter("can.interface").as_string(); subscriber_topic_name_ = this->get_parameter("topics.thruster_forces").as_string(); @@ -252,8 +249,8 @@ void ThrusterInterfaceAUVNode::extract_all_parameters() { if (thruster_count != 8) { spdlog::warn( - "UART packet format expects 8 thrusters, but config contains {} " - "entries", + "CAN FD thruster payload expects 8 thrusters, but config contains " + "{} entries", thruster_count); } From 19e12b1911c6ee5eb023da409947fc96c32cfc04 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Fri, 12 Jun 2026 15:14:22 +0200 Subject: [PATCH 2/3] fix: not having reset can id --- .../src/thruster_interface_auv_driver.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp index cfdeeda80..6f087f61e 100644 --- a/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp +++ b/motion/thruster_interface_auv/src/thruster_interface_auv_driver.cpp @@ -14,13 +14,14 @@ static constexpr std::uint32_t CAN_ID_DISABLE_THRUSTERS = 0x369U; static constexpr std::uint32_t CAN_ID_ENABLE_THRUSTERS = 0x36AU; -static constexpr std::uint32_t CAN_ID_SET_THRUSTERS_PWM = 0x36BU; -static constexpr std::uint32_t CAN_ID_SET_LIGHT_PWM = 0x36CU; - -static constexpr std::uint32_t CAN_ID_FLT_EVENT = 0x36DU; -static constexpr std::uint32_t CAN_ID_PGOOD_EVENT = 0x36EU; -static constexpr std::uint32_t CAN_ID_KILLSWITCH_EVENT = 0x36FU; -static constexpr std::uint32_t CAN_ID_CURRENT_MEASUREMENTS = 0x370U; +static constexpr std::uint32_t CAN_ID_RESET = 0x3BAU; +static constexpr std::uint32_t CAN_ID_SET_THRUSTERS_PWM = 0x36CU; +static constexpr std::uint32_t CAN_ID_SET_LIGHT_PWM = 0x36DU; + +static constexpr std::uint32_t CAN_ID_FLT_EVENT = 0x36EU; +static constexpr std::uint32_t CAN_ID_PGOOD_EVENT = 0x36FU; +static constexpr std::uint32_t CAN_ID_KILLSWITCH_EVENT = 0x370U; +static constexpr std::uint32_t CAN_ID_CURRENT_MEASUREMENTS = 0x371U; From 29a44362e89e284a1131cfccde6c6c91fa353298 Mon Sep 17 00:00:00 2001 From: forrisdahl Date: Tue, 16 Jun 2026 09:25:35 +0200 Subject: [PATCH 3/3] refactor: use can0 as default parameter --- .../config/thruster_interface_auv_config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml index 92ce27a46..860ec0722 100644 --- a/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml +++ b/motion/thruster_interface_auv/config/thruster_interface_auv_config.yaml @@ -21,7 +21,7 @@ # RIGHT: [1.33421, -18.75129, 121.29079, 1528.99886] can: - interface: "vcan0" + interface: "can0" debug: flag: True