From 79043dd5c7ca837da2ac9123792d8b1a6c8f7374 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Fri, 5 Sep 2025 13:48:57 +0200 Subject: [PATCH] boat: add module for rudder-steered boats --- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 21 +- .../px4fmu_common/init.d/rc.boat_rudder_apps | 8 + .../init.d/rc.boat_rudder_defaults | 11 + src/modules/boat_rudder/BoatRudder.cpp | 241 ++++++++++++++++++ src/modules/boat_rudder/BoatRudder.hpp | 130 ++++++++++ .../BoatRudderActControl.cpp | 125 +++++++++ .../BoatRudderActControl.hpp | 111 ++++++++ .../BoatRudderActControl/CMakeLists.txt | 38 +++ .../BoatRudderAttControl.cpp | 140 ++++++++++ .../BoatRudderAttControl.hpp | 126 +++++++++ .../BoatRudderAttControl/CMakeLists.txt | 39 +++ .../BoatRudderAutoMode/BoatRudderAutoMode.cpp | 157 ++++++++++++ .../BoatRudderAutoMode/BoatRudderAutoMode.hpp | 140 ++++++++++ .../BoatRudderAutoMode/CMakeLists.txt | 38 +++ .../BoatRudderManualMode.cpp | 227 +++++++++++++++++ .../BoatRudderManualMode.hpp | 134 ++++++++++ .../BoatRudderManualMode/CMakeLists.txt | 38 +++ .../BoatRudderOffboardMode.cpp | 82 ++++++ .../BoatRudderOffboardMode.hpp | 87 +++++++ .../BoatRudderOffboardMode/CMakeLists.txt | 38 +++ .../BoatRudderDriveModes/CMakeLists.txt | 36 +++ .../BoatRudderPosControl.cpp | 176 +++++++++++++ .../BoatRudderPosControl.hpp | 140 ++++++++++ .../BoatRudderPosControl/CMakeLists.txt | 39 +++ .../BoatRudderRateControl.cpp | 182 +++++++++++++ .../BoatRudderRateControl.hpp | 129 ++++++++++ .../BoatRudderRateControl/CMakeLists.txt | 39 +++ .../BoatRudderSpeedControl.cpp | 134 ++++++++++ .../BoatRudderSpeedControl.hpp | 128 ++++++++++ .../BoatRudderSpeedControl/CMakeLists.txt | 39 +++ src/modules/boat_rudder/CMakeLists.txt | 61 +++++ src/modules/boat_rudder/Kconfig | 5 + src/modules/boat_rudder/module.yaml | 69 +++++ 33 files changed, 3101 insertions(+), 7 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.boat_rudder_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.boat_rudder_defaults create mode 100644 src/modules/boat_rudder/BoatRudder.cpp create mode 100644 src/modules/boat_rudder/BoatRudder.hpp create mode 100644 src/modules/boat_rudder/BoatRudderActControl/BoatRudderActControl.cpp create mode 100644 src/modules/boat_rudder/BoatRudderActControl/BoatRudderActControl.hpp create mode 100644 src/modules/boat_rudder/BoatRudderActControl/CMakeLists.txt create mode 100644 src/modules/boat_rudder/BoatRudderAttControl/BoatRudderAttControl.cpp create mode 100644 src/modules/boat_rudder/BoatRudderAttControl/BoatRudderAttControl.hpp create mode 100644 src/modules/boat_rudder/BoatRudderAttControl/CMakeLists.txt create mode 100644 src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/BoatRudderAutoMode.cpp create mode 100644 src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/BoatRudderAutoMode.hpp create mode 100644 src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/CMakeLists.txt create mode 100644 src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/BoatRudderManualMode.cpp create mode 100644 src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/BoatRudderManualMode.hpp create mode 100644 src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/CMakeLists.txt create mode 100644 src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/BoatRudderOffboardMode.cpp create mode 100644 src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/BoatRudderOffboardMode.hpp create mode 100644 src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/CMakeLists.txt create mode 100644 src/modules/boat_rudder/BoatRudderDriveModes/CMakeLists.txt create mode 100644 src/modules/boat_rudder/BoatRudderPosControl/BoatRudderPosControl.cpp create mode 100644 src/modules/boat_rudder/BoatRudderPosControl/BoatRudderPosControl.hpp create mode 100644 src/modules/boat_rudder/BoatRudderPosControl/CMakeLists.txt create mode 100644 src/modules/boat_rudder/BoatRudderRateControl/BoatRudderRateControl.cpp create mode 100644 src/modules/boat_rudder/BoatRudderRateControl/BoatRudderRateControl.hpp create mode 100644 src/modules/boat_rudder/BoatRudderRateControl/CMakeLists.txt create mode 100644 src/modules/boat_rudder/BoatRudderSpeedControl/BoatRudderSpeedControl.cpp create mode 100644 src/modules/boat_rudder/BoatRudderSpeedControl/BoatRudderSpeedControl.hpp create mode 100644 src/modules/boat_rudder/BoatRudderSpeedControl/CMakeLists.txt create mode 100644 src/modules/boat_rudder/CMakeLists.txt create mode 100644 src/modules/boat_rudder/Kconfig create mode 100644 src/modules/boat_rudder/module.yaml diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 88c6be83a9..23b5c1d939 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -54,6 +54,13 @@ if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL) ) endif() +if(CONFIG_MODULES_BOAT_RUDDER) + px4_add_romfs_files( + rc.boat_rudder_apps + rc.boat_rudder_defaults + ) +endif() + if(CONFIG_MODULES_FW_RATE_CONTROL) px4_add_romfs_files( rc.fw_apps @@ -83,13 +90,6 @@ if(CONFIG_MODULES_ROVER_ACKERMANN) ) endif() -if(CONFIG_MODULES_SPACECRAFT) - px4_add_romfs_files( - rc.sc_apps - rc.sc_defaults - ) -endif() - if(CONFIG_MODULES_ROVER_MECANUM) px4_add_romfs_files( rc.rover_mecanum_apps @@ -97,6 +97,13 @@ if(CONFIG_MODULES_ROVER_MECANUM) ) endif() +if(CONFIG_MODULES_SPACECRAFT) + px4_add_romfs_files( + rc.sc_apps + rc.sc_defaults + ) +endif() + if(CONFIG_MODULES_UUV_ATT_CONTROL) px4_add_romfs_files( rc.uuv_apps diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_rudder_apps b/ROMFS/px4fmu_common/init.d/rc.boat_rudder_apps new file mode 100644 index 0000000000..28e4ef9e3d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.boat_rudder_apps @@ -0,0 +1,8 @@ +#!/bin/sh +# Standard apps for a boat rudder. + +# Start boat rudder module. +boat_rudder start + +# Start Land Detector. +land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_rudder_defaults b/ROMFS/px4fmu_common/init.d/rc.boat_rudder_defaults new file mode 100644 index 0000000000..d8d4c33259 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.boat_rudder_defaults @@ -0,0 +1,11 @@ +#!/bin/sh +# Rudder-steered boats parameters. + +set VEHICLE_TYPE boat +param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER +param set-default CA_AIRFRAME 5 # Rover (Ackermann) +param set-default CA_R_REV 1 # Motor is assumed to be reversible +param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying +param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 diff --git a/src/modules/boat_rudder/BoatRudder.cpp b/src/modules/boat_rudder/BoatRudder.cpp new file mode 100644 index 0000000000..33aeb97e01 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudder.cpp @@ -0,0 +1,241 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BoatRudder.hpp" + +using namespace time_literals; + +BoatRudder::BoatRudder() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); +} + +bool BoatRudder::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void BoatRudder::updateParams() +{ + ModuleParams::updateParams(); +} + +void BoatRudder::Run() +{ + if (_parameter_update_sub.updated()) { + parameter_update_s param_update{}; + _parameter_update_sub.copy(¶m_update); + updateParams(); + runSanityChecks(); + } + + if (_vehicle_control_mode_sub.updated()) { + vehicle_control_mode_s vehicle_control_mode{}; + _vehicle_control_mode_sub.copy(&vehicle_control_mode); + + // Run sanity checks if the control mode changes (Note: This has to be done this way, because the topic is periodically updated at 2 Hz) + if (_vehicle_control_mode.flag_control_position_enabled != vehicle_control_mode.flag_control_position_enabled || + _vehicle_control_mode.flag_control_velocity_enabled != vehicle_control_mode.flag_control_velocity_enabled || + _vehicle_control_mode.flag_control_attitude_enabled != vehicle_control_mode.flag_control_attitude_enabled || + _vehicle_control_mode.flag_control_rates_enabled != vehicle_control_mode.flag_control_rates_enabled || + _vehicle_control_mode.flag_control_allocation_enabled != vehicle_control_mode.flag_control_allocation_enabled) { + _vehicle_control_mode = vehicle_control_mode; + runSanityChecks(); + reset(); + + } else { + _vehicle_control_mode = vehicle_control_mode; + } + } + + if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) { + _was_armed = true; + generateSetpoints(); + updateControllers(); + + } else if (_was_armed) { // Reset all controllers and stop the vehicle + reset(); + _boat_rudder_act_control.stopVehicle(); + _was_armed = false; + } + +} + +void BoatRudder::generateSetpoints() +{ + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + + switch (vehicle_status.nav_state) { + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + _boat_rudder_auto_mode.autoControl(); + break; + + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + _boat_rudder_offboard_mode.offboardControl(); + break; + + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + _boat_rudder_manual_mode.manual(); + break; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: + _boat_rudder_manual_mode.acro(); + break; + + case vehicle_status_s::NAVIGATION_STATE_STAB: + _boat_rudder_manual_mode.stab(); + break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + _boat_rudder_manual_mode.position(); + break; + + default: + break; + } + +} + +void BoatRudder::updateControllers() +{ + if (_vehicle_control_mode.flag_control_position_enabled) { + _boat_rudder_pos_control.updatePosControl(); + } + + if (_vehicle_control_mode.flag_control_velocity_enabled) { + _boat_rudder_speed_control.updateSpeedControl(); + } + + if (_vehicle_control_mode.flag_control_attitude_enabled) { + _boat_rudder_att_control.updateAttControl(); + } + + if (_vehicle_control_mode.flag_control_rates_enabled) { + _boat_rudder_rate_control.updateRateControl(); + } + + if (_vehicle_control_mode.flag_control_allocation_enabled) { + _boat_rudder_act_control.updateActControl(); + } +} + +void BoatRudder::runSanityChecks() +{ + if (_vehicle_control_mode.flag_control_rates_enabled && !_boat_rudder_rate_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; + } + + if (_vehicle_control_mode.flag_control_attitude_enabled && !_boat_rudder_att_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; + } + + if (_vehicle_control_mode.flag_control_velocity_enabled && !_boat_rudder_speed_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; + } + + if (_vehicle_control_mode.flag_control_position_enabled && !_boat_rudder_pos_control.runSanityChecks()) { + _sanity_checks_passed = false; + return; + } + + _sanity_checks_passed = true; +} + +void BoatRudder::reset() +{ + _boat_rudder_pos_control.reset(); + _boat_rudder_speed_control.reset(); + _boat_rudder_att_control.reset(); + _boat_rudder_rate_control.reset(); + _boat_rudder_manual_mode.reset(); +} + +int BoatRudder::task_spawn(int argc, char *argv[]) +{ + BoatRudder *instance = new BoatRudder(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int BoatRudder::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int BoatRudder::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Module for rudder-steered boats. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("boat_rudder", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int boat_rudder_main(int argc, char *argv[]) +{ + return BoatRudder::main(argc, argv); +} diff --git a/src/modules/boat_rudder/BoatRudder.hpp b/src/modules/boat_rudder/BoatRudder.hpp new file mode 100644 index 0000000000..3318d1f070 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudder.hpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include +#include +#include +#include + +// Library includes +#include + +// uORB includes +#include +#include +#include +#include +#include + +// Local includes +#include "BoatRudderActControl/BoatRudderActControl.hpp" +#include "BoatRudderRateControl/BoatRudderRateControl.hpp" +#include "BoatRudderAttControl/BoatRudderAttControl.hpp" +#include "BoatRudderSpeedControl/BoatRudderSpeedControl.hpp" +#include "BoatRudderPosControl/BoatRudderPosControl.hpp" +#include "BoatRudderDriveModes/BoatRudderAutoMode/BoatRudderAutoMode.hpp" +#include "BoatRudderDriveModes/BoatRudderManualMode/BoatRudderManualMode.hpp" +#include "BoatRudderDriveModes/BoatRudderOffboardMode/BoatRudderOffboardMode.hpp" + +class BoatRudder : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + BoatRudder(); + ~BoatRudder() override = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +protected: + void updateParams() override; + +private: + void Run() override; + + /** + * @brief Generate surface vehicle setpoints if the vehicle is in a + * supported PX4 internal mode. + * Note: The surface vehicle setpoints are expected to be published from outside this module + * if the vehicle is not in a PX4 internal mode. + */ + void generateSetpoints(); + + void updateControllers(); + + /** + * @brief Check proper parameter setup for the controllers + * + * Modifies: + * + * - _sanity_checks_passed: true if checks for all active controllers pass + */ + void runSanityChecks(); + + /** + * @brief Reset controllers and manual mode variables. + */ + void reset(); + + // uORB subscriptions + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + vehicle_control_mode_s _vehicle_control_mode{}; + + // Class instances + BoatRudderActControl _boat_rudder_act_control{this}; + BoatRudderRateControl _boat_rudder_rate_control{this}; + BoatRudderAttControl _boat_rudder_att_control{this}; + BoatRudderSpeedControl _boat_rudder_speed_control{this}; + BoatRudderPosControl _boat_rudder_pos_control{this}; + BoatRudderAutoMode _boat_rudder_auto_mode{this}; + BoatRudderManualMode _boat_rudder_manual_mode{this}; + BoatRudderOffboardMode _boat_rudder_offboard_mode{this}; + + // Variables + bool _sanity_checks_passed{true}; // True if checks for all active controllers pass + bool _was_armed{false}; // True if the vehicle was armed before the last reset +}; diff --git a/src/modules/boat_rudder/BoatRudderActControl/BoatRudderActControl.cpp b/src/modules/boat_rudder/BoatRudderActControl/BoatRudderActControl.cpp new file mode 100644 index 0000000000..7486f2a9da --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderActControl/BoatRudderActControl.cpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BoatRudderActControl.hpp" + +using namespace time_literals; + +BoatRudderActControl::BoatRudderActControl(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); +} + +void BoatRudderActControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_br_str_rate_limit.get() > FLT_EPSILON && _param_br_max_str_ang.get() > FLT_EPSILON) { + _servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_br_str_rate_limit.get()) / _param_br_max_str_ang.get()); + } + + if (_param_sv_accel_limit.get() > FLT_EPSILON && _param_sv_max_thr_speed.get() > FLT_EPSILON) { + _motor_setpoint.setSlewRate(_param_sv_accel_limit.get() / _param_sv_max_thr_speed.get()); + } +} + +void BoatRudderActControl::updateActControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f; + + // Motor control + if (_surface_vehicle_throttle_setpoint_sub.updated()) { + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + _surface_vehicle_throttle_setpoint_sub.copy(&surface_vehicle_throttle_setpoint); + _throttle_setpoint = surface_vehicle_throttle_setpoint.throttle_body_x; + } + + if (PX4_ISFINITE(_throttle_setpoint)) { + actuator_motors_s actuator_motors_sub{}; + _actuator_motors_sub.copy(&actuator_motors_sub); + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = SurfaceVehicleControl::throttleControl(_motor_setpoint, + _throttle_setpoint, actuator_motors_sub.control[0], _param_sv_accel_limit.get(), + _param_sv_decel_limit.get(), _param_sv_max_thr_speed.get(), dt); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + + } + + // Servo control + if (_surface_vehicle_steering_setpoint_sub.updated()) { + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + _surface_vehicle_steering_setpoint_sub.copy(&surface_vehicle_steering_setpoint); + _steering_setpoint = surface_vehicle_steering_setpoint.normalized_steering_setpoint; + } + + if (PX4_ISFINITE(_steering_setpoint)) { + actuator_servos_s actuator_servos_sub{}; + _actuator_servos_sub.copy(&actuator_servos_sub); + + if (_param_br_str_rate_limit.get() > FLT_EPSILON + && _param_br_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured + if (fabsf(_servo_setpoint.getState() - actuator_servos_sub.control[0]) > fabsf( + _steering_setpoint - + actuator_servos_sub.control[0])) { + _servo_setpoint.setForcedValue(actuator_servos_sub.control[0]); + } + + _servo_setpoint.update(_steering_setpoint, dt); + + } else { + _servo_setpoint.setForcedValue(_steering_setpoint); + } + + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = _servo_setpoint.getState(); + actuator_servos.timestamp = _timestamp; + _actuator_servos_pub.publish(actuator_servos); + } +} + +void BoatRudderActControl::stopVehicle() +{ + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = 0.f; + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = 0.f; + actuator_servos.timestamp = _timestamp; + _actuator_servos_pub.publish(actuator_servos); +} diff --git a/src/modules/boat_rudder/BoatRudderActControl/BoatRudderActControl.hpp b/src/modules/boat_rudder/BoatRudderActControl/BoatRudderActControl.hpp new file mode 100644 index 0000000000..d8de1400e7 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderActControl/BoatRudderActControl.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include + +// Libraries +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for actuator control of rudder-steered boats. + */ +class BoatRudderActControl : public ModuleParams +{ +public: + /** + * @brief Constructor for BoatRudderActControl. + * @param parent The parent ModuleParams object. + */ + BoatRudderActControl(ModuleParams *parent); + ~BoatRudderActControl() = default; + + /** + * @brief Generate and publish actuatorMotors/actuatorServos setpoints from SurfaceVehicleThrottleSetpoint/SurfaceVehicleSteeringSetpoint. + */ + void updateActControl(); + + /** + * @brief Stop the vehicle by sending 0 commands to motors and servos. + */ + void stopVehicle(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + // uORB subscriptions + uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _surface_vehicle_steering_setpoint_sub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Subscription _surface_vehicle_throttle_setpoint_sub{ORB_ID(surface_vehicle_throttle_setpoint)}; + + // uORB publications + uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + + // Variables + hrt_abstime _timestamp{0}; + float _throttle_setpoint{NAN}; + float _steering_setpoint{NAN}; + + // Controllers + SlewRate _servo_setpoint{0.f}; + SlewRate _motor_setpoint{0.f}; + + // Parameters + DEFINE_PARAMETERS( + (ParamInt) _param_r_rev, + (ParamFloat) _param_br_str_rate_limit, + (ParamFloat) _param_br_max_str_ang, + (ParamFloat) _param_sv_accel_limit, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_max_thr_speed + ) +}; diff --git a/src/modules/boat_rudder/BoatRudderActControl/CMakeLists.txt b/src/modules/boat_rudder/BoatRudderActControl/CMakeLists.txt new file mode 100644 index 0000000000..df1a8470fc --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderActControl/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(BoatRudderActControl + BoatRudderActControl.cpp +) + +target_include_directories(BoatRudderActControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/boat_rudder/BoatRudderAttControl/BoatRudderAttControl.cpp b/src/modules/boat_rudder/BoatRudderAttControl/BoatRudderAttControl.cpp new file mode 100644 index 0000000000..7e3b636df3 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderAttControl/BoatRudderAttControl.cpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BoatRudderAttControl.hpp" + +using namespace time_literals; + +BoatRudderAttControl::BoatRudderAttControl(ModuleParams *parent) : ModuleParams(parent) +{ + _surface_vehicle_rate_setpoint_pub.advertise(); + _surface_vehicle_attitude_status_pub.advertise(); + updateParams(); +} + +void BoatRudderAttControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_sv_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + } + + // Set up PID controller + _pid_yaw.setGains(_param_sv_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setIntegralLimit(0.f); + _pid_yaw.setOutputLimit(_max_yaw_rate); + + // Set up slew rate + _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); +} + +void BoatRudderAttControl::updateAttControl() +{ + updateSubscriptions(); + + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f; + + if (PX4_ISFINITE(_yaw_setpoint)) { + // Calculate yaw rate limit for slew rate + float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_br_max_str_ang.get()) / + _param_br_wheel_base.get(); // Maximum possible yaw rate at current velocity + float yaw_slew_rate = math::min(max_possible_yaw_rate, _max_yaw_rate); + + float yaw_rate_setpoint = SurfaceVehicleControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, yaw_slew_rate, + _vehicle_yaw, _yaw_setpoint, dt); + + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = _timestamp; + surface_vehicle_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); + + } + + // Publish attitude controller status (logging only) + surface_vehicle_attitude_status_s surface_vehicle_attitude_status; + surface_vehicle_attitude_status.timestamp = _timestamp; + surface_vehicle_attitude_status.measured_yaw = _vehicle_yaw; + surface_vehicle_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState()); + _surface_vehicle_attitude_status_pub.publish(surface_vehicle_attitude_status); + +} + +void BoatRudderAttControl::updateSubscriptions() +{ + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + // Estimate forward speed based on throttle + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors; + _actuator_motors_sub.copy(&actuator_motors); + _estimated_speed_body_x = math::interpolate (actuator_motors.control[0], -1.f, 1.f, + -_param_sv_max_thr_speed.get(), _param_sv_max_thr_speed.get()); + } + + if (_surface_vehicle_attitude_setpoint_sub.updated()) { + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + _surface_vehicle_attitude_setpoint_sub.copy(&surface_vehicle_attitude_setpoint); + _yaw_setpoint = surface_vehicle_attitude_setpoint.yaw_setpoint; + } +} + +bool BoatRudderAttControl::runSanityChecks() +{ + bool ret = true; + + if (_param_sv_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_br_wheel_base.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_br_max_str_ang.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_sv_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + return ret; +} diff --git a/src/modules/boat_rudder/BoatRudderAttControl/BoatRudderAttControl.hpp b/src/modules/boat_rudder/BoatRudderAttControl/BoatRudderAttControl.hpp new file mode 100644 index 0000000000..38cb668d67 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderAttControl/BoatRudderAttControl.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include + +// Library includes +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for attitude control of rudder-steered boats. + */ +class BoatRudderAttControl : public ModuleParams +{ +public: + /** + * @brief Constructor for BoatRudderAttControl. + * @param parent The parent ModuleParams object. + */ + BoatRudderAttControl(ModuleParams *parent); + ~BoatRudderAttControl() = default; + + /** + * @brief Generate and publish SurfaceVehicleRateSetpoint from SurfaceVehicleAttitudeSetpoint. + */ + void updateAttControl(); + + /** + * @brief Reset attitude controller. + */ + void reset() {_pid_yaw.resetIntegral(); _yaw_setpoint = NAN;}; + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in attitude controller. + */ + void updateSubscriptions(); + + // uORB subscriptions + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _surface_vehicle_attitude_setpoint_sub{ORB_ID(surface_vehicle_attitude_setpoint)}; + + // uORB publications + uORB::Publication _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)}; + uORB::Publication _surface_vehicle_attitude_status_pub{ORB_ID(surface_vehicle_attitude_status)}; + + // Variables + float _vehicle_yaw{0.f}; + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed_body_x] + between [0, 0] and [1, _param_sv_max_thr_speed].*/ + float _yaw_setpoint{NAN}; + + // Controllers + PID _pid_yaw; + SlewRateYaw _adjusted_yaw_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_sv_max_thr_speed, + (ParamFloat) _param_br_wheel_base, + (ParamFloat) _param_br_max_str_ang, + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_p, + (ParamFloat) _param_sv_yaw_stick_dz + ) +}; diff --git a/src/modules/boat_rudder/BoatRudderAttControl/CMakeLists.txt b/src/modules/boat_rudder/BoatRudderAttControl/CMakeLists.txt new file mode 100644 index 0000000000..4e32569803 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderAttControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(BoatRudderAttControl + BoatRudderAttControl.cpp +) + +target_link_libraries(BoatRudderAttControl PUBLIC PID) +target_include_directories(BoatRudderAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/BoatRudderAutoMode.cpp b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/BoatRudderAutoMode.cpp new file mode 100644 index 0000000000..762432bd64 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/BoatRudderAutoMode.cpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BoatRudderAutoMode.hpp" + +using namespace time_literals; + +BoatRudderAutoMode::BoatRudderAutoMode(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _surface_vehicle_position_setpoint_pub.advertise(); +} + +void BoatRudderAutoMode::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + + if (_param_br_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON + && _param_br_max_str_ang.get() > FLT_EPSILON) { + _min_speed = _param_br_wheel_base.get() * _max_yaw_rate / tanf(_param_br_max_str_ang.get()); + } +} + +void BoatRudderAutoMode::autoControl() +{ + if (_position_setpoint_triplet_sub.updated()) { + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + } + + updateWaypointsAndAcceptanceRadius(); + + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = _curr_wp_ned(0); + surface_vehicle_position_setpoint.position_ned[1] = _curr_wp_ned(1); + surface_vehicle_position_setpoint.start_ned[0] = _prev_wp_ned(0); + surface_vehicle_position_setpoint.start_ned[1] = _prev_wp_ned(1); + surface_vehicle_position_setpoint.arrival_speed = arrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, + _curr_wp_type, + _waypoint_transition_angle, _max_yaw_rate); + surface_vehicle_position_setpoint.cruising_speed = _cruising_speed; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); + } + +} + +void BoatRudderAutoMode::updateWaypointsAndAcceptanceRadius() +{ + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + _curr_wp_type = position_setpoint_triplet.current.type; + + SurfaceVehicleControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _global_ned_proj_ref); + + _waypoint_transition_angle = SurfaceVehicleControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, + _next_wp_ned); + + // Update acceptance radius + if (_param_br_acc_rad_max.get() >= _param_nav_acc_rad.get()) { + _acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(), + _param_br_acc_rad_gain.get(), _param_br_acc_rad_max.get(), _param_br_wheel_base.get(), _param_br_max_str_ang.get()); + + } else { + _acceptance_radius = _param_nav_acc_rad.get(); + } + + // Waypoint cruising speed + _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_sv_speed_limit.get()) : _param_sv_speed_limit.get(); +} + +float BoatRudderAutoMode::updateAcceptanceRadius(const float waypoint_transition_angle, + const float default_acceptance_radius, const float acceptance_radius_gain, + const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) +{ + // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment + float acceptance_radius = default_acceptance_radius; + + if (PX4_ISFINITE(_waypoint_transition_angle)) { + const float theta = waypoint_transition_angle / 2.f; + const float min_turning_radius = wheel_base / sinf(max_steer_angle); + const float acceptance_radius_temp = min_turning_radius / tanf(theta); + const float acceptance_radius_temp_scaled = acceptance_radius_gain * + acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects + acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, + acceptance_radius_max); + } + + // Publish updated acceptance radius + position_controller_status_s pos_ctrl_status{}; + pos_ctrl_status.acceptance_radius = acceptance_radius; + pos_ctrl_status.timestamp = hrt_absolute_time(); + _position_controller_status_pub.publish(pos_ctrl_status); + return acceptance_radius; +} + +float BoatRudderAutoMode::arrivalSpeed(const float cruising_speed, const float min_speed, const float acc_rad, + const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate) +{ + if (!PX4_ISFINITE(waypoint_transition_angle) + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + return 0.f; // Stop at the waypoint + + } else if (_param_sv_speed_red.get() > FLT_EPSILON) { + const float speed_reduction = math::constrain(_param_sv_speed_red.get() * math::interpolate( + M_PI_F - waypoint_transition_angle, + 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); + return math::constrain(_param_sv_max_thr_speed.get() * (1.f - speed_reduction), min_speed, + cruising_speed); // Slow down for cornering + } + + return cruising_speed; // Fallthrough +} diff --git a/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/BoatRudderAutoMode.hpp b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/BoatRudderAutoMode.hpp new file mode 100644 index 0000000000..f083ac4252 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/BoatRudderAutoMode.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include + +// Libraries +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for auto mode for rudder-steered boats. + */ +class BoatRudderAutoMode : public ModuleParams +{ +public: + /** + * @brief Constructor for auto mode. + * @param parent The parent ModuleParams object. + */ + BoatRudderAutoMode(ModuleParams *parent); + ~BoatRudderAutoMode() = default; + + /** + * @brief Generate and publish SurfaceVehiclePositionSetpoint from positionSetpointTriplet. + */ + void autoControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update global/NED waypoint coordinates and acceptance radius. + */ + void updateWaypointsAndAcceptanceRadius(); + + /** + * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment + * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param default_acceptance_radius Default acceptance radius for waypoints [m]. + * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. + * @param acceptance_radius_max Maximum value for the acceptance radius [m]. + * @param wheel_base Rover wheelbase [m]. + * @param max_steer_angle Rover maximum steer angle [rad]. + * @return Updated acceptance radius [m]. + */ + float updateAcceptanceRadius(float waypoint_transition_angle, float default_acceptance_radius, + float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle); + + /** + * @brief Calculate the speed at which the rover should arrive at the current waypoint based on the upcoming corner. + * @param cruising_speed Cruising speed [m/s]. + * @param min_speed Minimum speed setpoint [m/s]. + * @param acc_rad Acceptance radius of the current waypoint [m]. + * @param curr_wp_type Type of the current waypoint. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_yaw_rate Maximum yaw rate setpoint [rad/s] + * @return Speed setpoint [m/s]. + */ + float arrivalSpeed(float cruising_speed, float min_speed, float acc_rad, int curr_wp_type, + float waypoint_transition_angle, float max_yaw_rate); + + // uORB subscriptions + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + + // uORB publications + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + + // Variables + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + Vector2f _curr_wp_ned{NAN, NAN}; + Vector2f _prev_wp_ned{NAN, NAN}; + Vector2f _next_wp_ned{NAN, NAN}; + Vector2f _curr_pos_ned{NAN, NAN}; + float _acceptance_radius{0.5f}; + float _cruising_speed{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + float _max_yaw_rate{NAN}; + float _min_speed{NAN}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base. + int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_sv_speed_limit, + (ParamFloat) _param_br_wheel_base, + (ParamFloat) _param_br_max_str_ang, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_br_acc_rad_max, + (ParamFloat) _param_br_acc_rad_gain, + (ParamFloat) _param_sv_speed_red, + (ParamFloat) _param_sv_max_thr_speed + ) +}; diff --git a/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/CMakeLists.txt b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/CMakeLists.txt new file mode 100644 index 0000000000..b5b9c0a935 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(BoatRudderAutoMode + BoatRudderAutoMode.cpp +) + +target_include_directories(BoatRudderAutoMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/BoatRudderManualMode.cpp b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/BoatRudderManualMode.cpp new file mode 100644 index 0000000000..6e152f3f76 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/BoatRudderManualMode.cpp @@ -0,0 +1,227 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BoatRudderManualMode.hpp" + +using namespace time_literals; + +BoatRudderManualMode::BoatRudderManualMode(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _surface_vehicle_throttle_setpoint_pub.advertise(); + _surface_vehicle_steering_setpoint_pub.advertise(); + _surface_vehicle_rate_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); + _surface_vehicle_speed_setpoint_pub.advertise(); + _surface_vehicle_position_setpoint_pub.advertise(); +} + +void BoatRudderManualMode::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; +} + +void BoatRudderManualMode::manual() +{ + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + surface_vehicle_steering_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_steering_setpoint.normalized_steering_setpoint = manual_control_setpoint.roll; + _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); +} + +void BoatRudderManualMode::acro() +{ + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate * + math::superexpo + (manual_control_setpoint.roll, _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); +} + +void BoatRudderManualMode::stab() +{ + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); + + if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON + || fabsf(surface_vehicle_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { + _stab_yaw_setpoint = NAN; + + // Rate control + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate * + math::superexpo(math::deadzone(manual_control_setpoint.roll, + _param_sv_yaw_stick_dz.get()), _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); + + // Set uncontrolled setpoint invalid + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = NAN; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); + + } else { // Heading control + if (!PX4_ISFINITE(_stab_yaw_setpoint)) { + _stab_yaw_setpoint = _vehicle_yaw; + } + + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); + } +} + +void BoatRudderManualMode::position() +{ + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + } + + manual_control_setpoint_s manual_control_setpoint{}; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + const float speed_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_sv_speed_limit.get(), _param_sv_speed_limit.get()); + + if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON + || fabsf(speed_setpoint) < FLT_EPSILON) { + _pos_ctl_course_direction = Vector2f(NAN, NAN); + + // Speed control + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_speed_setpoint.speed_body_x = speed_setpoint; + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + + // Rate control + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate * + math::superexpo(math::deadzone(manual_control_setpoint.roll, + _param_sv_yaw_stick_dz.get()), _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); + _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); + + // Set uncontrolled setpoints invalid + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = NAN; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); + + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = NAN; + surface_vehicle_position_setpoint.position_ned[1] = NAN; + surface_vehicle_position_setpoint.start_ned[0] = NAN; + surface_vehicle_position_setpoint.start_ned[1] = NAN; + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.cruising_speed = NAN; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); + + } else { // Course control + if (!_pos_ctl_course_direction.isAllFinite()) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned; + const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(speed_setpoint) * + vector_scaling * _pos_ctl_course_direction; + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = target_waypoint_ned(0); + surface_vehicle_position_setpoint.position_ned[1] = target_waypoint_ned(1); + surface_vehicle_position_setpoint.start_ned[0] = _pos_ctl_start_position_ned(0); + surface_vehicle_position_setpoint.start_ned[1] = _pos_ctl_start_position_ned(1); + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.cruising_speed = speed_setpoint; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); + } +} + +void BoatRudderManualMode::reset() +{ + _stab_yaw_setpoint = NAN; + _pos_ctl_course_direction = Vector2f(NAN, NAN); + _pos_ctl_start_position_ned = Vector2f(NAN, NAN); + _curr_pos_ned = Vector2f(NAN, NAN); +} diff --git a/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/BoatRudderManualMode.hpp b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/BoatRudderManualMode.hpp new file mode 100644 index 0000000000..96992fdd00 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/BoatRudderManualMode.hpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include + +// Libraries +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for manual modes for rudder-steered boats. + */ +class BoatRudderManualMode : public ModuleParams +{ +public: + /** + * @brief Constructor for BoatRudderManualMode. + * @param parent The parent ModuleParams object. + */ + BoatRudderManualMode(ModuleParams *parent); + ~BoatRudderManualMode() = default; + + /** + * @brief Publish SurfaceVehicleThrottleSetpoint and SurfaceVehicleSteeringSetpoint from manualControlSetpoint. + */ + void manual(); + + /** + * @brief Generate and publish SurfaceVehicleThrottleSetpoint and SurfaceVehicleRateSetpoint from manualControlSetpoint. + */ + void acro(); + + /** + * @brief Generate and publish SurfaceVehicleThrottleSetpoint and SurfaceVehicleAttitudeSetpoint from manualControlSetpoint. + */ + void stab(); + + /** + * @brief Generate and publish SurfaceVehicleSpeedSetpoint/SurfaceVehicleRateSetpoint or SurfaceVehiclePositionSetpoint from manualControlSetpoint. + */ + void position(); + + /** + * @brief Reset manual mode variables. + */ + void reset(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + // uORB subscriptions + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + + // uORB publications + uORB::Publication _surface_vehicle_throttle_setpoint_pub{ORB_ID(surface_vehicle_throttle_setpoint)}; + uORB::Publication _surface_vehicle_steering_setpoint_pub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Publication _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; + + // Variables + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + Quatf _vehicle_attitude_quaternion{}; + Vector2f _pos_ctl_course_direction{NAN, NAN}; + Vector2f _pos_ctl_start_position_ned{NAN, NAN}; + Vector2f _curr_pos_ned{NAN, NAN}; + float _stab_yaw_setpoint{NAN}; + float _vehicle_yaw{NAN}; + float _max_yaw_rate{NAN}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_p, + (ParamFloat) _param_sv_yaw_stick_dz, + (ParamFloat) _param_sv_yaw_expo, + (ParamFloat) _param_sv_yaw_supexpo, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_sv_speed_limit + ) +}; diff --git a/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/CMakeLists.txt b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/CMakeLists.txt new file mode 100644 index 0000000000..576c39ed82 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(BoatRudderManualMode + BoatRudderManualMode.cpp +) + +target_include_directories(BoatRudderManualMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/BoatRudderOffboardMode.cpp b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/BoatRudderOffboardMode.cpp new file mode 100644 index 0000000000..42b9554d3d --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/BoatRudderOffboardMode.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BoatRudderOffboardMode.hpp" + +using namespace time_literals; + +BoatRudderOffboardMode::BoatRudderOffboardMode(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _surface_vehicle_speed_setpoint_pub.advertise(); + _surface_vehicle_position_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); +} + +void BoatRudderOffboardMode::updateParams() +{ + ModuleParams::updateParams(); +} + +void BoatRudderOffboardMode::offboardControl() +{ + offboard_control_mode_s offboard_control_mode{}; + _offboard_control_mode_sub.copy(&offboard_control_mode); + + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (offboard_control_mode.position) { + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; + surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_position_setpoint.position_ned[0] = trajectory_setpoint.position[0]; + surface_vehicle_position_setpoint.position_ned[1] = trajectory_setpoint.position[1]; + surface_vehicle_position_setpoint.start_ned[0] = NAN; + surface_vehicle_position_setpoint.start_ned[1] = NAN; + surface_vehicle_position_setpoint.cruising_speed = NAN; + surface_vehicle_position_setpoint.arrival_speed = NAN; + surface_vehicle_position_setpoint.yaw = NAN; + _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); + + } else if (offboard_control_mode.velocity) { + const Vector2f velocity_ned(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_speed_setpoint.speed_body_x = velocity_ned.norm(); + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + surface_vehicle_attitude_setpoint.yaw_setpoint = atan2f(velocity_ned(1), velocity_ned(0)); + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); + } +} diff --git a/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/BoatRudderOffboardMode.hpp b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/BoatRudderOffboardMode.hpp new file mode 100644 index 0000000000..1dca03eba5 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/BoatRudderOffboardMode.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include + +// Libraries +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for offboard mode for rudder-steered boats. + */ +class BoatRudderOffboardMode : public ModuleParams +{ +public: + /** + * @brief Constructor for BoatRudderOffboardMode. + * @param parent The parent ModuleParams object. + */ + BoatRudderOffboardMode(ModuleParams *parent); + ~BoatRudderOffboardMode() = default; + + /** + * @brief Generate and publish roverSetpoints from trajectorySetpoint. + */ + void offboardControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + // uORB subscriptions + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + + // uORB publications + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; +}; diff --git a/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/CMakeLists.txt b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/CMakeLists.txt new file mode 100644 index 0000000000..c4ae5fb224 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(BoatRudderOffboardMode + BoatRudderOffboardMode.cpp +) + +target_include_directories(BoatRudderOffboardMode PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/boat_rudder/BoatRudderDriveModes/CMakeLists.txt b/src/modules/boat_rudder/BoatRudderDriveModes/CMakeLists.txt new file mode 100644 index 0000000000..bbfe50642d --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderDriveModes/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(BoatRudderAutoMode) +add_subdirectory(BoatRudderManualMode) +add_subdirectory(BoatRudderOffboardMode) diff --git a/src/modules/boat_rudder/BoatRudderPosControl/BoatRudderPosControl.cpp b/src/modules/boat_rudder/BoatRudderPosControl/BoatRudderPosControl.cpp new file mode 100644 index 0000000000..56cfba5fd2 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderPosControl/BoatRudderPosControl.cpp @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BoatRudderPosControl.hpp" + +using namespace time_literals; + +BoatRudderPosControl::BoatRudderPosControl(ModuleParams *parent) : ModuleParams(parent) +{ + _pure_pursuit_status_pub.advertise(); + _surface_vehicle_speed_setpoint_pub.advertise(); + _surface_vehicle_attitude_setpoint_pub.advertise(); + + updateParams(); +} + +void BoatRudderPosControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _min_speed = _param_br_wheel_base.get() * _max_yaw_rate / tanf(_param_br_max_str_ang.get()); + +} + +void BoatRudderPosControl::updatePosControl() +{ + updateSubscriptions(); + + hrt_abstime timestamp = hrt_absolute_time(); + + if (_target_waypoint_ned.isAllFinite()) { + float distance_to_target = (_target_waypoint_ned - _curr_pos_ned).norm(); + + if (_arrival_speed > FLT_EPSILON) { + distance_to_target -= _acceptance_radius; // shift target to the edge of the acceptance radius if arrival speed not zero + } + + if (distance_to_target > _acceptance_radius || _arrival_speed > FLT_EPSILON) { + + float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_sv_jerk_limit.get(), + _param_sv_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); + speed_setpoint = math::min(speed_setpoint, _cruising_speed); + + pure_pursuit_status_s pure_pursuit_status{}; + pure_pursuit_status.timestamp = timestamp; + + const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(), + _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _target_waypoint_ned, _start_ned, + _curr_pos_ned, fabsf(speed_setpoint)); + + if (_param_sv_speed_red.get() > FLT_EPSILON) { + const float course_error = fabsf(matrix::wrap_pi(bearing_setpoint - _vehicle_yaw)); + const float speed_reduction = math::constrain(_param_sv_speed_red.get() * math::interpolate(course_error, + 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); + const float max_speed = math::constrain(_param_sv_max_thr_speed.get() * (1.f - speed_reduction), _min_speed, + _param_sv_max_thr_speed.get()); + speed_setpoint = math::constrain(speed_setpoint, -max_speed, max_speed); + } + + _pure_pursuit_status_pub.publish(pure_pursuit_status); + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = timestamp; + surface_vehicle_speed_setpoint.speed_body_x = speed_setpoint; + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = timestamp; + surface_vehicle_attitude_setpoint.yaw_setpoint = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi( + bearing_setpoint + M_PI_F); + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); + + } else { + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint{}; + surface_vehicle_speed_setpoint.timestamp = timestamp; + surface_vehicle_speed_setpoint.speed_body_x = 0.f; + _surface_vehicle_speed_setpoint_pub.publish(surface_vehicle_speed_setpoint); + surface_vehicle_attitude_setpoint_s surface_vehicle_attitude_setpoint{}; + surface_vehicle_attitude_setpoint.timestamp = timestamp; + surface_vehicle_attitude_setpoint.yaw_setpoint = _vehicle_yaw; + _surface_vehicle_attitude_setpoint_pub.publish(surface_vehicle_attitude_setpoint); + + if (!_stopped && fabsf(_vehicle_speed) < FLT_EPSILON) { + _stopped = true; + _target_waypoint_ned = _curr_pos_ned; + } + + if (_stopped && _updated_reset_counter != _reset_counter) { + _target_waypoint_ned = _curr_pos_ned; + _reset_counter = _updated_reset_counter; + } + } + } +} + +void BoatRudderPosControl::updateSubscriptions() +{ + if (_position_controller_status_sub.updated()) { + position_controller_status_s position_controller_status{}; + _position_controller_status_sub.copy(&position_controller_status); + _acceptance_radius = position_controller_status.acceptance_radius; + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + _updated_reset_counter = vehicle_local_position.xy_reset_counter; + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); + Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); + _vehicle_speed = velocity_2d.norm() > _param_sv_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; + } + + if (_surface_vehicle_position_setpoint_sub.updated()) { + surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint; + _surface_vehicle_position_setpoint_sub.copy(&surface_vehicle_position_setpoint); + _start_ned = Vector2f(surface_vehicle_position_setpoint.start_ned[0], surface_vehicle_position_setpoint.start_ned[1]); + _start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned; + _arrival_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.arrival_speed) ? + surface_vehicle_position_setpoint.arrival_speed : 0.f; + _cruising_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.cruising_speed) ? + surface_vehicle_position_setpoint.cruising_speed : + _param_sv_speed_limit.get(); + _target_waypoint_ned = Vector2f(surface_vehicle_position_setpoint.position_ned[0], + surface_vehicle_position_setpoint.position_ned[1]); + _stopped = false; + } + +} + +bool BoatRudderPosControl::runSanityChecks() +{ + bool ret = true; + + if (_param_sv_speed_limit.get() < FLT_EPSILON) { + ret = false; + } + + return ret; +} diff --git a/src/modules/boat_rudder/BoatRudderPosControl/BoatRudderPosControl.hpp b/src/modules/boat_rudder/BoatRudderPosControl/BoatRudderPosControl.hpp new file mode 100644 index 0000000000..e4454956a8 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderPosControl/BoatRudderPosControl.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Library includes +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for position control of rudder-steered boats. + */ +class BoatRudderPosControl : public ModuleParams +{ +public: + /** + * @brief Constructor for BoatRudderPosControl. + * @param parent The parent ModuleParams object. + */ + BoatRudderPosControl(ModuleParams *parent); + ~BoatRudderPosControl() = default; + + /** + * @brief Generate and publish SurfaceVehicleSpeedSetpoint and SurfaceVehicleAttitudeSetpoint from SurfaceVehiclePositionSetpoint. + */ + void updatePosControl(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + /** + * @brief Reset position controller. + */ + void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_sv_speed_limit.get(); _stopped = false;}; + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in position controller. + */ + void updateSubscriptions(); + + // uORB subscriptions + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _surface_vehicle_position_setpoint_sub{ORB_ID(surface_vehicle_position_setpoint)}; + uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)}; + + // uORB publications + uORB::Publication _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)}; + uORB::Publication _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)}; + uORB::Publication _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)}; + + // Variables + Quatf _vehicle_attitude_quaternion{}; + Vector2f _curr_pos_ned{}; + Vector2f _start_ned{}; + Vector2f _target_waypoint_ned{}; + float _arrival_speed{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + float _acceptance_radius{0.f}; // Acceptance radius for the waypoint. + float _min_speed{NAN}; + float _vehicle_speed{0.f}; + float _cruising_speed{NAN}; + bool _stopped{false}; + uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ + uint8_t _updated_reset_counter{0}; /**< counter for estimator resets in xy-direction */ + + DEFINE_PARAMETERS( + (ParamFloat) _param_sv_max_thr_speed, + (ParamFloat) _param_sv_speed_red, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_jerk_limit, + (ParamFloat) _param_sv_speed_limit, + (ParamFloat) _param_pp_lookahd_gain, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_pp_lookahd_min, + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_br_wheel_base, + (ParamFloat) _param_br_max_str_ang, + (ParamFloat) _param_sv_speed_th + ) +}; diff --git a/src/modules/boat_rudder/BoatRudderPosControl/CMakeLists.txt b/src/modules/boat_rudder/BoatRudderPosControl/CMakeLists.txt new file mode 100644 index 0000000000..fb3232fb7e --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderPosControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(BoatRudderPosControl + BoatRudderPosControl.cpp +) + +target_link_libraries(BoatRudderPosControl PUBLIC PID) +target_include_directories(BoatRudderPosControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/boat_rudder/BoatRudderRateControl/BoatRudderRateControl.cpp b/src/modules/boat_rudder/BoatRudderRateControl/BoatRudderRateControl.cpp new file mode 100644 index 0000000000..2fe6315a48 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderRateControl/BoatRudderRateControl.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BoatRudderRateControl.hpp" + +using namespace time_literals; + +BoatRudderRateControl::BoatRudderRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _surface_vehicle_steering_setpoint_pub.advertise(); + _surface_vehicle_rate_status_pub.advertise(); + updateParams(); +} + +void BoatRudderRateControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + + // Set up PID controller + _pid_yaw_rate.setGains(_param_sv_yaw_rate_p.get(), _param_sv_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + + // Set up slew rate + _adjusted_yaw_rate_setpoint.setSlewRate(_param_sv_yaw_accel_limit.get() * M_DEG_TO_RAD_F); +} + +void BoatRudderRateControl::updateRateControl() +{ + updateSubscriptions(); + + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f; + + if (PX4_ISFINITE(_yaw_rate_setpoint)) { + if (fabsf(_estimated_speed) > FLT_EPSILON) { + // Set up feasible yaw rate setpoint + float steering_setpoint{0.f}; + float max_possible_yaw_rate = fabsf(_estimated_speed) * tanf(_param_br_max_str_ang.get()) / + _param_br_wheel_base.get(); // Maximum possible yaw rate at current velocity + float yaw_rate_limit = math::min(max_possible_yaw_rate, _max_yaw_rate); + float constrained_yaw_rate = math::constrain(_yaw_rate_setpoint, -yaw_rate_limit, yaw_rate_limit); + + if (_param_sv_yaw_accel_limit.get() > FLT_EPSILON) { // Apply slew rate if configured + if (fabsf(_adjusted_yaw_rate_setpoint.getState() - _vehicle_yaw_rate) > fabsf(constrained_yaw_rate - + _vehicle_yaw_rate)) { + _adjusted_yaw_rate_setpoint.setForcedValue(_vehicle_yaw_rate); + } + + _adjusted_yaw_rate_setpoint.update(constrained_yaw_rate, dt); + + } else { + _adjusted_yaw_rate_setpoint.setForcedValue(constrained_yaw_rate); + } + + // Feed forward + steering_setpoint = atanf(_adjusted_yaw_rate_setpoint.getState() * _param_br_wheel_base.get() / _estimated_speed) * + _param_sv_yaw_rate_corr.get(); + + // Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability) + if (_estimated_speed > FLT_EPSILON) { + _pid_yaw_rate.setSetpoint(_adjusted_yaw_rate_setpoint.getState()); + steering_setpoint += _pid_yaw_rate.update(_vehicle_yaw_rate, dt); + } + + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + surface_vehicle_steering_setpoint.timestamp = _timestamp; + surface_vehicle_steering_setpoint.normalized_steering_setpoint = math::interpolate(steering_setpoint, + -_param_br_max_str_ang.get(), _param_br_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint + _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); + + } else { + _pid_yaw_rate.resetIntegral(); + surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; + surface_vehicle_steering_setpoint.timestamp = _timestamp; + surface_vehicle_steering_setpoint.normalized_steering_setpoint = 0.f; + _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); + } + } + + + // Publish rate controller status (logging only) + surface_vehicle_rate_status_s surface_vehicle_rate_status; + surface_vehicle_rate_status.timestamp = _timestamp; + surface_vehicle_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + surface_vehicle_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); + surface_vehicle_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _surface_vehicle_rate_status_pub.publish(surface_vehicle_rate_status); + +} + +void BoatRudderRateControl::updateSubscriptions() +{ + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_sv_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + vehicle_angular_velocity.xyz[2] : 0.f; + } + + // Estimate forward speed based on throttle + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors; + _actuator_motors_sub.copy(&actuator_motors); + _estimated_speed = math::interpolate(actuator_motors.control[0], -1.f, 1.f, + -_param_sv_max_thr_speed.get(), _param_sv_max_thr_speed.get()); + _estimated_speed = fabsf(_estimated_speed) > _param_sv_speed_th.get() ? _estimated_speed : 0.f; + } + + if (_surface_vehicle_rate_setpoint_sub.updated()) { + surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; + _surface_vehicle_rate_setpoint_sub.copy(&surface_vehicle_rate_setpoint); + _yaw_rate_setpoint = surface_vehicle_rate_setpoint.yaw_rate_setpoint; + } +} + +bool BoatRudderRateControl::runSanityChecks() +{ + bool ret = true; + + if (_param_sv_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + events::send(events::ID("boat_rudder_rate_control_conf_invalid_max_thr_speed"), events::Log::Error, + "Invalid configuration of necessary parameter SV_MAX_THR_SPEED", _param_sv_max_thr_speed.get()); + + } + + if (_param_br_wheel_base.get() < FLT_EPSILON) { + ret = false; + events::send(events::ID("boat_rudder_rate_control_conf_invalid_wheel_base"), events::Log::Error, + "Invalid configuration of necessary parameter RA_WHEEL_BASE", _param_br_wheel_base.get()); + + } + + if (_param_br_max_str_ang.get() < FLT_EPSILON) { + ret = false; + events::send(events::ID("boat_rudder_rate_control_conf_invalid_max_str_ang"), events::Log::Error, + "Invalid configuration of necessary parameter RA_MAX_STR_ANG", _param_br_max_str_ang.get()); + + } + + if (_param_sv_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + events::send(events::ID("boat_rudder_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, + "Invalid configuration of necessary parameter SV_YAW_RATE_LIM", _param_sv_yaw_rate_limit.get()); + + } + + return ret; +} diff --git a/src/modules/boat_rudder/BoatRudderRateControl/BoatRudderRateControl.hpp b/src/modules/boat_rudder/BoatRudderRateControl/BoatRudderRateControl.hpp new file mode 100644 index 0000000000..80eaf558e3 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderRateControl/BoatRudderRateControl.hpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for rate control of rudder-steered boats. + */ +class BoatRudderRateControl : public ModuleParams +{ +public: + /** + * @brief Constructor for BoatRudderRateControl. + * @param parent The parent ModuleParams object. + */ + BoatRudderRateControl(ModuleParams *parent); + ~BoatRudderRateControl() = default; + + /** + * @brief Generate and publish SurfaceVehicleSteeringSetpoint from SurfaceVehicleRateSetpoint. + */ + void updateRateControl(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + /** + * @brief Reset rate controller. + */ + void reset() {_pid_yaw_rate.resetIntegral(); _yaw_rate_setpoint = NAN;}; + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in rate controller. + */ + void updateSubscriptions(); + + // uORB subscriptions + uORB::Subscription _surface_vehicle_rate_setpoint_sub{ORB_ID(surface_vehicle_rate_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + + // uORB publications + uORB::Publication _surface_vehicle_steering_setpoint_pub{ORB_ID(surface_vehicle_steering_setpoint)}; + uORB::Publication _surface_vehicle_rate_status_pub{ORB_ID(surface_vehicle_rate_status)}; + + // Variables + float _estimated_speed{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed] + between [0, 0] and [1, _param_sv_max_thr_speed].*/ + float _max_yaw_rate{0.f}; + float _vehicle_yaw_rate{0.f}; + float _yaw_rate_setpoint{NAN}; + hrt_abstime _timestamp{0}; + + // Controllers + PID _pid_yaw_rate; + SlewRate _adjusted_yaw_rate_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sv_max_thr_speed, + (ParamFloat) _param_br_wheel_base, + (ParamFloat) _param_br_max_str_ang, + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_rate_th, + (ParamFloat) _param_sv_yaw_rate_p, + (ParamFloat) _param_sv_yaw_rate_i, + (ParamFloat) _param_sv_yaw_accel_limit, + (ParamFloat) _param_sv_speed_th, + (ParamFloat) _param_sv_yaw_rate_corr + ) +}; diff --git a/src/modules/boat_rudder/BoatRudderRateControl/CMakeLists.txt b/src/modules/boat_rudder/BoatRudderRateControl/CMakeLists.txt new file mode 100644 index 0000000000..18fba185e8 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderRateControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(BoatRudderRateControl + BoatRudderRateControl.cpp +) + +target_link_libraries(BoatRudderRateControl PUBLIC PID) +target_include_directories(BoatRudderRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/boat_rudder/BoatRudderSpeedControl/BoatRudderSpeedControl.cpp b/src/modules/boat_rudder/BoatRudderSpeedControl/BoatRudderSpeedControl.cpp new file mode 100644 index 0000000000..ff7d3b1b81 --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderSpeedControl/BoatRudderSpeedControl.cpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BoatRudderSpeedControl.hpp" + +using namespace time_literals; + +BoatRudderSpeedControl::BoatRudderSpeedControl(ModuleParams *parent) : ModuleParams(parent) +{ + _surface_vehicle_throttle_setpoint_pub.advertise(); + _surface_vehicle_speed_status_pub.advertise(); + updateParams(); +} + +void BoatRudderSpeedControl::updateParams() +{ + ModuleParams::updateParams(); + + // Set up PID controller + _pid_speed.setGains(_param_sv_speed_p.get(), _param_sv_speed_i.get(), 0.f); + _pid_speed.setIntegralLimit(1.f); + _pid_speed.setOutputLimit(1.f); + + // Set up slew rate + if (_param_sv_accel_limit.get() > FLT_EPSILON) { + _adjusted_speed_setpoint.setSlewRate(_param_sv_accel_limit.get()); + } +} + +void BoatRudderSpeedControl::updateSpeedControl() +{ + updateSubscriptions(); + + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 10_ms) * 1e-6f; + + // Throttle Setpoint + if (PX4_ISFINITE(_speed_setpoint)) { + const float speed_setpoint = math::constrain(_speed_setpoint, -_param_sv_speed_limit.get(), + _param_sv_speed_limit.get()); + surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; + surface_vehicle_throttle_setpoint.timestamp = _timestamp; + surface_vehicle_throttle_setpoint.throttle_body_x = SurfaceVehicleControl::speedControl(_adjusted_speed_setpoint, + _pid_speed, + speed_setpoint, _vehicle_speed, _param_sv_accel_limit.get(), _param_sv_decel_limit.get(), + _param_sv_max_thr_speed.get(), dt); + surface_vehicle_throttle_setpoint.throttle_body_y = NAN; + _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); + } + + // Publish speed controller status (logging only) + surface_vehicle_speed_status_s surface_vehicle_speed_status; + surface_vehicle_speed_status.timestamp = _timestamp; + surface_vehicle_speed_status.measured_speed_body_x = _vehicle_speed; + surface_vehicle_speed_status.adjusted_speed_body_x_setpoint = _adjusted_speed_setpoint.getState(); + surface_vehicle_speed_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + surface_vehicle_speed_status.measured_speed_body_y = NAN; + surface_vehicle_speed_status.adjusted_speed_body_y_setpoint = NAN; + surface_vehicle_speed_status.pid_throttle_body_y_integral = NAN; + _surface_vehicle_speed_status_pub.publish(surface_vehicle_speed_status); +} + +void BoatRudderSpeedControl::updateSubscriptions() +{ + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); + Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); + _vehicle_speed = velocity_2d.norm() > _param_sv_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; + } + + if (_surface_vehicle_speed_setpoint_sub.updated()) { + surface_vehicle_speed_setpoint_s surface_vehicle_speed_setpoint; + _surface_vehicle_speed_setpoint_sub.copy(&surface_vehicle_speed_setpoint); + _speed_setpoint = surface_vehicle_speed_setpoint.speed_body_x; + } +} + +bool BoatRudderSpeedControl::runSanityChecks() +{ + bool ret = true; + + if (_param_sv_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_sv_speed_limit.get() < FLT_EPSILON) { + ret = false; + events::send(events::ID("boat_rudder_speed_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter SV_SPEED_LIM", _param_sv_speed_limit.get()); + + } + + return ret; +} diff --git a/src/modules/boat_rudder/BoatRudderSpeedControl/BoatRudderSpeedControl.hpp b/src/modules/boat_rudder/BoatRudderSpeedControl/BoatRudderSpeedControl.hpp new file mode 100644 index 0000000000..df60b27b3a --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderSpeedControl/BoatRudderSpeedControl.hpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Library includes +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for speed control of rudder-steered boats. + */ +class BoatRudderSpeedControl : public ModuleParams +{ +public: + /** + * @brief Constructor for BoatRudderSpeedControl. + * @param parent The parent ModuleParams object. + */ + BoatRudderSpeedControl(ModuleParams *parent); + ~BoatRudderSpeedControl() = default; + + /** + * @brief Generate and publish SurfaceVehicleThrottleSetpoint from SurfaceVehicleSpeedSetpoint. + */ + void updateSpeedControl(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + /** + * @brief Reset speed controller. + */ + void reset() {_pid_speed.resetIntegral(); _speed_setpoint = NAN; _adjusted_speed_setpoint.setForcedValue(0.f);}; + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in speed controller. + */ + void updateSubscriptions(); + + // uORB subscriptions + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _surface_vehicle_speed_setpoint_sub{ORB_ID(surface_vehicle_speed_setpoint)}; + + // uORB publications + uORB::Publication _surface_vehicle_throttle_setpoint_pub{ORB_ID(surface_vehicle_throttle_setpoint)}; + uORB::Publication _surface_vehicle_speed_status_pub{ORB_ID(surface_vehicle_speed_status)}; + + // Variables + hrt_abstime _timestamp{0}; + Quatf _vehicle_attitude_quaternion{}; + float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards + float _speed_setpoint{NAN}; + + // Controllers + PID _pid_speed; + SlewRate _adjusted_speed_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sv_max_thr_speed, + (ParamFloat) _param_sv_speed_p, + (ParamFloat) _param_sv_speed_i, + (ParamFloat) _param_sv_accel_limit, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_jerk_limit, + (ParamFloat) _param_sv_speed_limit, + (ParamFloat) _param_sv_speed_th + ) +}; diff --git a/src/modules/boat_rudder/BoatRudderSpeedControl/CMakeLists.txt b/src/modules/boat_rudder/BoatRudderSpeedControl/CMakeLists.txt new file mode 100644 index 0000000000..304844d08b --- /dev/null +++ b/src/modules/boat_rudder/BoatRudderSpeedControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(BoatRudderSpeedControl + BoatRudderSpeedControl.cpp +) + +target_link_libraries(BoatRudderSpeedControl PUBLIC PID) +target_include_directories(BoatRudderSpeedControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/boat_rudder/CMakeLists.txt b/src/modules/boat_rudder/CMakeLists.txt new file mode 100644 index 0000000000..854c67a383 --- /dev/null +++ b/src/modules/boat_rudder/CMakeLists.txt @@ -0,0 +1,61 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(BoatRudderActControl) +add_subdirectory(BoatRudderRateControl) +add_subdirectory(BoatRudderAttControl) +add_subdirectory(BoatRudderSpeedControl) +add_subdirectory(BoatRudderPosControl) +add_subdirectory(BoatRudderDriveModes) + +px4_add_module( + MODULE modules__boat_rudder + MAIN boat_rudder + SRCS + BoatRudder.cpp + BoatRudder.hpp + DEPENDS + BoatRudderActControl + BoatRudderRateControl + BoatRudderAttControl + BoatRudderSpeedControl + BoatRudderPosControl + BoatRudderAutoMode + BoatRudderManualMode + BoatRudderOffboardMode + px4_work_queue + surface_vehicle_control + pure_pursuit + MODULE_CONFIG + module.yaml +) diff --git a/src/modules/boat_rudder/Kconfig b/src/modules/boat_rudder/Kconfig new file mode 100644 index 0000000000..d084bf2cca --- /dev/null +++ b/src/modules/boat_rudder/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_BOAT_RUDDER + bool "boat_rudder" + default n + ---help--- + Enable support for rudder-steered boats diff --git a/src/modules/boat_rudder/module.yaml b/src/modules/boat_rudder/module.yaml new file mode 100644 index 0000000000..99d1ef2e52 --- /dev/null +++ b/src/modules/boat_rudder/module.yaml @@ -0,0 +1,69 @@ +module_name: Boat Rudder + +parameters: + - group: Boat Rudder + definitions: + BR_WHEEL_BASE: + description: + short: Wheel base + long: Distance from the front to the rear axle. + type: float + unit: m + min: 0 + max: 100 + increment: 0.001 + decimal: 3 + default: 0 + + BR_MAX_STR_ANG: + description: + short: Maximum steering angle + type: float + unit: rad + min: 0 + max: 1.5708 + increment: 0.01 + decimal: 2 + default: 0 + + BR_STR_RATE_LIM: + description: + short: Steering rate limit + long: Set to -1 to disable. + type: float + unit: deg/s + min: -1 + max: 1000 + increment: 0.01 + decimal: 2 + default: -1 + + BR_ACC_RAD_MAX: + description: + short: Maximum acceptance radius for the waypoints + long: | + The controller scales the acceptance radius based on the angle between + the previous, current and next waypoint. + Higher value -> smoother trajectory at the cost of how close the rover gets + to the waypoint (Set to -1 to disable corner cutting). + type: float + unit: m + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + BR_ACC_RAD_GAIN: + description: + short: Tuning parameter for corner cutting + long: | + The geometric ideal acceptance radius is multiplied by this factor + to account for kinematic and dynamic effects. + Higher value -> The rover starts to cut the corner earlier. + type: float + min: 1 + max: 100 + increment: 0.01 + decimal: 2 + default: 1