diff --git a/thruster_hardware/README.md b/thruster_hardware/README.md index a623d6c..188ec44 100644 --- a/thruster_hardware/README.md +++ b/thruster_hardware/README.md @@ -2,7 +2,7 @@ thruster_hardware implements a ros2_control system hardware interface for individual thruster-level control. This is accomplished by dynamically setting -the ArduSub `SERVON_FUNCTION` parameters for each thruster to PWM passthrough +the ArduSub `SERVON_FUNCTION` parameters for each thruster to RCIN mode using MAVROS. > [!IMPORTANT] @@ -10,7 +10,7 @@ using MAVROS. > *after* MAVROS has fully loaded the "param", "command", and "rc_io" plugins. > [!CAUTION] -> Please exercise caution when using PWM passthrough. This mode disables all +> Please exercise caution when using RCIN mode. This mode disables all > ArduSub arming checks. It is also recommended that you store a backup of the > ArduSub parameters should the hardware interface fail to restore the default > parameters. @@ -30,6 +30,10 @@ thruster_hardware/ThrusterHardware * default_param_value: The default value of the servo function parameter. The thruster hardware will attempt to restore these parameters on deactivation. [integer] -* channel: The thruster channel number. [integer] +* channel: The thruster channel number. It is recommended to avoid using + channels 7 through 11, as these channels may conflict with other vehicle + functionalities. [integer] +* desired_param_value: The desired value for the servo function parameter + (e.g., `51` corresponds to `RCIN1`). [integer] * neutral_pwm (optional): The neutral PWM value for the thruster. This defaults to 1500. [integer] diff --git a/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp b/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp index ebdbd16..e0513cf 100644 --- a/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp +++ b/thruster_hardware/include/thruster_hardware/thruster_hardware.hpp @@ -64,6 +64,7 @@ class ThrusterHardware : public hardware_interface::SystemInterface rcl_interfaces::msg::Parameter param; int channel; int neutral_pwm = 1500; + int desired_value; }; auto stop_thrusters() -> void; diff --git a/thruster_hardware/src/thruster_hardware.cpp b/thruster_hardware/src/thruster_hardware.cpp index 82eaf33..e871572 100644 --- a/thruster_hardware/src/thruster_hardware.cpp +++ b/thruster_hardware/src/thruster_hardware.cpp @@ -66,12 +66,21 @@ auto ThrusterHardware::on_init(const hardware_interface::HardwareInfo & info) -> } const auto default_value = std::stoi(default_value_it->second); + const auto desired_value_it = joint.parameters.find("desired_param_value"); + if (desired_value_it == joint.parameters.cend()) { + // NOLINTNEXTLINE + RCLCPP_ERROR(logger_, "Joint %s is missing the required parameter 'desired_param_value'", joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + const auto desired_value = std::stoi(desired_value_it->second); + // store the thruster configurations ThrusterConfig config; config.param.name = param_name; config.param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; config.param.value.integer_value = default_value; config.channel = channel; + config.desired_value = desired_value; if (joint.parameters.find("neutral_pwm") != joint.parameters.cend()) { config.neutral_pwm = std::stoi(joint.parameters.at("neutral_pwm")); @@ -143,7 +152,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st rcl_interfaces::msg::Parameter param; param.name = config.param.name; param.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - param.value.integer_value = 1; // Set the thruster parameter values to RC passthrough here + param.value.integer_value = config.desired_value; // Set the thruster parameter values to the desired values here params.emplace_back(param); } @@ -151,7 +160,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st request->parameters = params; for (int i = 0; i < max_retries_; ++i) { - RCLCPP_WARN(logger_, "Attempting to set thruster parameters to RC passthrough..."); // NOLINT + RCLCPP_WARN(logger_, "Attempting to set thruster parameters to RCIN mode..."); // NOLINT // Wait until the result is available auto future = set_params_client_->async_send_request(request); @@ -164,7 +173,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st } } - RCLCPP_INFO(logger_, "Successfully set thruster parameters to RC passthrough!"); // NOLINT + RCLCPP_INFO(logger_, "Successfully set thruster parameters to RCIN mode!"); // NOLINT // Stop the thrusters before switching to an external controller stop_thrusters(); @@ -178,7 +187,7 @@ auto ThrusterHardware::on_activate(const rclcpp_lifecycle::State & /*previous_st RCLCPP_ERROR( // NOLINT logger_, - "Failed to set thruster parameters to passthrough mode after %d attempts. Make sure that the MAVROS parameter " + "Failed to set thruster parameters to RCIN mode after %d attempts. Make sure that the MAVROS parameter " "plugin is fully running and configured.", max_retries_); @@ -194,7 +203,7 @@ auto ThrusterHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_ // stop sending rc override messages from the write loop is_active_ = false; - // stop the thrusters before switching out of passthrough mode + // stop the thrusters before switching out of RCIN mode stop_thrusters(); std::vector params; @@ -208,7 +217,7 @@ auto ThrusterHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_ request->parameters = params; for (int i = 0; i < max_retries_; ++i) { - RCLCPP_WARN(logger_, "Attempting to leave RC passthrough mode..."); // NOLINT + RCLCPP_WARN(logger_, "Attempting to leave RCIN mode..."); // NOLINT auto future = set_params_client_->async_send_request(request); @@ -230,7 +239,7 @@ auto ThrusterHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_ RCLCPP_ERROR( // NOLINT logger_, - "Failed to fully leave passthrough mode after %d attempts. Make sure that the MAVROS parameter plugin is fully " + "Failed to fully leave RCIN mode after %d attempts. Make sure that the MAVROS parameter plugin is fully " "running and configured.", max_retries_);