fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 11:04:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

boat: add module for rudder-steered boats

Browse Source
This commit is contained in:
chfriedrich98 2025-09-05 13:48:57 +02:00
parent 9c38602c12
commit 79043dd5c7
33 changed files with 3101 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ROMFS/px4fmu_common/init.d/CMakeLists.txt
View File

@ -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

8
ROMFS/px4fmu_common/init.d/rc.boat_rudder_apps Normal file
View File

@ -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

11
ROMFS/px4fmu_common/init.d/rc.boat_rudder_defaults Normal file
View File

@ -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

241
src/modules/boat_rudder/BoatRudder.cpp Normal file
View File

@ -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(&param_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);
}

130
src/modules/boat_rudder/BoatRudder.hpp Normal file
View File

@ -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 <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
// Library includes
#include <math.h>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
// 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<BoatRudder>, 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
};

125
src/modules/boat_rudder/BoatRudderActControl/BoatRudderActControl.cpp Normal file
View File

@ -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);
}

111
src/modules/boat_rudder/BoatRudderActControl/BoatRudderActControl.hpp Normal file
View File

@ -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 <px4_platform_common/module_params.h>
// Libraries
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <math.h>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/surface_vehicle_steering_setpoint.h>
#include <uORB/topics/surface_vehicle_throttle_setpoint.h>
/**
* @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_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
// Variables
hrt_abstime _timestamp{0};
float _throttle_setpoint{NAN};
float _steering_setpoint{NAN};
// Controllers
SlewRate<float> _servo_setpoint{0.f};
SlewRate<float> _motor_setpoint{0.f};
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(ParamFloat<px4::params::BR_STR_RATE_LIM>) _param_br_str_rate_limit,
(ParamFloat<px4::params::BR_MAX_STR_ANG>) _param_br_max_str_ang,
(ParamFloat<px4::params::SV_ACCEL_LIM>) _param_sv_accel_limit,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed
)
};

38
src/modules/boat_rudder/BoatRudderActControl/CMakeLists.txt Normal file
View File

@ -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})

140
src/modules/boat_rudder/BoatRudderAttControl/BoatRudderAttControl.cpp Normal file
View File

@ -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<float> (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;
}

126
src/modules/boat_rudder/BoatRudderAttControl/BoatRudderAttControl.hpp Normal file
View File

@ -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 <px4_platform_common/module_params.h>
// Library includes
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <lib/slew_rate/SlewRateYaw.hpp>
#include <math.h>
#include <matrix/matrix/math.hpp>
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/surface_vehicle_rate_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/surface_vehicle_attitude_status.h>
#include <uORB/topics/surface_vehicle_attitude_setpoint.h>
#include <uORB/topics/actuator_motors.h>
/**
* @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_s> _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)};
uORB::Publication<surface_vehicle_attitude_status_s> _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<float> _adjusted_yaw_setpoint;
// Parameters
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::BR_WHEEL_BASE>) _param_br_wheel_base,
(ParamFloat<px4::params::BR_MAX_STR_ANG>) _param_br_max_str_ang,
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_P>) _param_sv_yaw_p,
(ParamFloat<px4::params::SV_YAW_STICK_DZ>) _param_sv_yaw_stick_dz
)
};

39
src/modules/boat_rudder/BoatRudderAttControl/CMakeLists.txt Normal file
View File

@ -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})

157
src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/BoatRudderAutoMode.cpp Normal file
View File

@ -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<float>(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
}

140
src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/BoatRudderAutoMode.hpp Normal file
View File

@ -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 <px4_platform_common/module_params.h>
// Libraries
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <math.h>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/surface_vehicle_position_setpoint.h>
/**
* @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_s> _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)};
uORB::Publication<position_controller_status_s> _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<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::BR_WHEEL_BASE>) _param_br_wheel_base,
(ParamFloat<px4::params::BR_MAX_STR_ANG>) _param_br_max_str_ang,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::BR_ACC_RAD_MAX>) _param_br_acc_rad_max,
(ParamFloat<px4::params::BR_ACC_RAD_GAIN>) _param_br_acc_rad_gain,
(ParamFloat<px4::params::SV_SPEED_RED>) _param_sv_speed_red,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed
)
};

38
src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderAutoMode/CMakeLists.txt Normal file
View File

@ -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})

227
src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/BoatRudderManualMode.cpp Normal file
View File

@ -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<float>
(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<float>(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<float>(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<float>(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);
}

134
src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/BoatRudderManualMode.hpp Normal file
View File

@ -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 <px4_platform_common/module_params.h>
// Libraries
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <math.h>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/surface_vehicle_throttle_setpoint.h>
#include <uORB/topics/surface_vehicle_steering_setpoint.h>
#include <uORB/topics/surface_vehicle_rate_setpoint.h>
#include <uORB/topics/surface_vehicle_attitude_setpoint.h>
#include <uORB/topics/surface_vehicle_speed_setpoint.h>
#include <uORB/topics/surface_vehicle_position_setpoint.h>
/**
* @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_s> _surface_vehicle_throttle_setpoint_pub{ORB_ID(surface_vehicle_throttle_setpoint)};
uORB::Publication<surface_vehicle_steering_setpoint_s> _surface_vehicle_steering_setpoint_pub{ORB_ID(surface_vehicle_steering_setpoint)};
uORB::Publication<surface_vehicle_rate_setpoint_s> _surface_vehicle_rate_setpoint_pub{ORB_ID(surface_vehicle_rate_setpoint)};
uORB::Publication<surface_vehicle_attitude_setpoint_s> _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)};
uORB::Publication<surface_vehicle_speed_setpoint_s> _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)};
uORB::Publication<surface_vehicle_position_setpoint_s> _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<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_P>) _param_sv_yaw_p,
(ParamFloat<px4::params::SV_YAW_STICK_DZ>) _param_sv_yaw_stick_dz,
(ParamFloat<px4::params::SV_YAW_EXPO>) _param_sv_yaw_expo,
(ParamFloat<px4::params::SV_YAW_SUPEXPO>) _param_sv_yaw_supexpo,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit
)
};

38
src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderManualMode/CMakeLists.txt Normal file
View File

@ -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})

82
src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/BoatRudderOffboardMode.cpp Normal file
View File

@ -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);
}
}

87
src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/BoatRudderOffboardMode.hpp Normal file
View File

@ -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 <px4_platform_common/module_params.h>
// Libraries
#include <math.h>
#include <matrix/matrix/math.hpp>
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/surface_vehicle_speed_setpoint.h>
#include <uORB/topics/surface_vehicle_attitude_setpoint.h>
#include <uORB/topics/surface_vehicle_position_setpoint.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/trajectory_setpoint.h>
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_s> _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)};
uORB::Publication<surface_vehicle_position_setpoint_s> _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)};
uORB::Publication<surface_vehicle_attitude_setpoint_s> _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)};
};

38
src/modules/boat_rudder/BoatRudderDriveModes/BoatRudderOffboardMode/CMakeLists.txt Normal file
View File

@ -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})

36
src/modules/boat_rudder/BoatRudderDriveModes/CMakeLists.txt Normal file
View File

@ -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)

176
src/modules/boat_rudder/BoatRudderPosControl/BoatRudderPosControl.cpp Normal file
View File

@ -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;
}

140
src/modules/boat_rudder/BoatRudderPosControl/BoatRudderPosControl.hpp Normal file
View File

@ -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 <px4_platform_common/module_params.h>
#include <px4_platform_common/events.h>
// Library includes
#include <matrix/matrix/math.hpp>
#include <lib/pure_pursuit/PurePursuit.hpp>
#include <math.h>
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/surface_vehicle_position_setpoint.h>
#include <uORB/topics/surface_vehicle_speed_setpoint.h>
#include <uORB/topics/surface_vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/pure_pursuit_status.h>
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_s> _surface_vehicle_speed_setpoint_pub{ORB_ID(surface_vehicle_speed_setpoint)};
uORB::Publication<surface_vehicle_attitude_setpoint_s> _surface_vehicle_attitude_setpoint_pub{ORB_ID(surface_vehicle_attitude_setpoint)};
uORB::Publication<pure_pursuit_status_s> _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<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::SV_SPEED_RED>) _param_sv_speed_red,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_JERK_LIM>) _param_sv_jerk_limit,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::BR_WHEEL_BASE>) _param_br_wheel_base,
(ParamFloat<px4::params::BR_MAX_STR_ANG>) _param_br_max_str_ang,
(ParamFloat<px4::params::SV_SPEED_TH>) _param_sv_speed_th
)
};

39
src/modules/boat_rudder/BoatRudderPosControl/CMakeLists.txt Normal file
View File

@ -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})

182
src/modules/boat_rudder/BoatRudderRateControl/BoatRudderRateControl.cpp Normal file
View File

@ -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<float>(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<float>(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<float>(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<float>(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<float>(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<float>(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;
}

129
src/modules/boat_rudder/BoatRudderRateControl/BoatRudderRateControl.hpp Normal file
View File

@ -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 <px4_platform_common/module_params.h>
#include <px4_platform_common/events.h>
// Libraries
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <math.h>
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/surface_vehicle_rate_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/surface_vehicle_steering_setpoint.h>
#include <uORB/topics/surface_vehicle_rate_status.h>
#include <uORB/topics/actuator_motors.h>
/**
* @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_s> _surface_vehicle_steering_setpoint_pub{ORB_ID(surface_vehicle_steering_setpoint)};
uORB::Publication<surface_vehicle_rate_status_s> _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<float> _adjusted_yaw_rate_setpoint{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::BR_WHEEL_BASE>) _param_br_wheel_base,
(ParamFloat<px4::params::BR_MAX_STR_ANG>) _param_br_max_str_ang,
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_RATE_TH>) _param_sv_yaw_rate_th,
(ParamFloat<px4::params::SV_YAW_RATE_P>) _param_sv_yaw_rate_p,
(ParamFloat<px4::params::SV_YAW_RATE_I>) _param_sv_yaw_rate_i,
(ParamFloat<px4::params::SV_YAW_ACCEL_LIM>) _param_sv_yaw_accel_limit,
(ParamFloat<px4::params::SV_SPEED_TH>) _param_sv_speed_th,
(ParamFloat<px4::params::SV_YAW_RATE_CORR>) _param_sv_yaw_rate_corr
)
};

39
src/modules/boat_rudder/BoatRudderRateControl/CMakeLists.txt Normal file
View File

@ -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})

134
src/modules/boat_rudder/BoatRudderSpeedControl/BoatRudderSpeedControl.cpp Normal file
View File

@ -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<float>(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;
}

128
src/modules/boat_rudder/BoatRudderSpeedControl/BoatRudderSpeedControl.hpp Normal file
View File

@ -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 <px4_platform_common/module_params.h>
#include <px4_platform_common/events.h>
// Library includes
#include <lib/surface_vehicle_control/SurfaceVehicleControl.hpp>
#include <lib/pid/PID.hpp>
#include <matrix/matrix/math.hpp>
#include <lib/slew_rate/SlewRate.hpp>
#include <math.h>
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/surface_vehicle_throttle_setpoint.h>
#include <uORB/topics/surface_vehicle_speed_setpoint.h>
#include <uORB/topics/surface_vehicle_speed_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
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_s> _surface_vehicle_throttle_setpoint_pub{ORB_ID(surface_vehicle_throttle_setpoint)};
uORB::Publication<surface_vehicle_speed_status_s> _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<float> _adjusted_speed_setpoint{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::SV_SPEED_P>) _param_sv_speed_p,
(ParamFloat<px4::params::SV_SPEED_I>) _param_sv_speed_i,
(ParamFloat<px4::params::SV_ACCEL_LIM>) _param_sv_accel_limit,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_JERK_LIM>) _param_sv_jerk_limit,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::SV_SPEED_TH>) _param_sv_speed_th
)
};

39
src/modules/boat_rudder/BoatRudderSpeedControl/CMakeLists.txt Normal file
View File

@ -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})

61
src/modules/boat_rudder/CMakeLists.txt Normal file
View File

@ -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
)

5
src/modules/boat_rudder/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig MODULES_BOAT_RUDDER
bool "boat_rudder"
default n
---help---
Enable support for rudder-steered boats

69
src/modules/boat_rudder/module.yaml Normal file
View File

@ -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