From cae77467caddd50b2b3b40d524805a567cab6326 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Mon, 13 May 2024 14:17:34 +0200 Subject: [PATCH] Boat Module --- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 9 +- .../px4fmu_common/init.d/airframes/70001_boat | 51 +++++ ROMFS/px4fmu_common/init.d/rc.boat_apps | 11 + ROMFS/px4fmu_common/init.d/rc.boat_defaults | 2 +- ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 9 + boards/px4/fmu-v5x/boat.px4board | 17 ++ boards/px4/fmu-v6x/boat.px4board | 17 ++ msg/BoatSetpoint.msg | 8 + msg/CMakeLists.txt | 1 + src/modules/boat/Boat.cpp | 194 +++++++++++++++++ src/modules/boat/Boat.hpp | 107 ++++++++++ src/modules/boat/BoatControl/BoatControl.cpp | 110 ++++++++++ src/modules/boat/BoatControl/BoatControl.hpp | 101 +++++++++ src/modules/boat/BoatControl/CMakeLists.txt | 39 ++++ .../boat/BoatGuidance/BoatGuidance.cpp | 154 ++++++++++++++ .../boat/BoatGuidance/BoatGuidance.hpp | 155 ++++++++++++++ src/modules/boat/BoatGuidance/CMakeLists.txt | 38 ++++ .../boat/BoatKinematics/BoatKinematics.cpp | 89 ++++++++ .../boat/BoatKinematics/BoatKinematics.hpp | 106 +++++++++ .../boat/BoatKinematics/CMakeLists.txt | 40 ++++ src/modules/boat/CMakeLists.txt | 53 +++++ src/modules/boat/Kconfig | 6 + src/modules/boat/module.yaml | 201 ++++++++++++++++++ 23 files changed, 1515 insertions(+), 3 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/70001_boat create mode 100644 ROMFS/px4fmu_common/init.d/rc.boat_apps create mode 100644 boards/px4/fmu-v5x/boat.px4board create mode 100644 boards/px4/fmu-v6x/boat.px4board create mode 100644 msg/BoatSetpoint.msg create mode 100644 src/modules/boat/Boat.cpp create mode 100644 src/modules/boat/Boat.hpp create mode 100644 src/modules/boat/BoatControl/BoatControl.cpp create mode 100644 src/modules/boat/BoatControl/BoatControl.hpp create mode 100644 src/modules/boat/BoatControl/CMakeLists.txt create mode 100644 src/modules/boat/BoatGuidance/BoatGuidance.cpp create mode 100644 src/modules/boat/BoatGuidance/BoatGuidance.hpp create mode 100644 src/modules/boat/BoatGuidance/CMakeLists.txt create mode 100644 src/modules/boat/BoatKinematics/BoatKinematics.cpp create mode 100644 src/modules/boat/BoatKinematics/BoatKinematics.hpp create mode 100644 src/modules/boat/BoatKinematics/CMakeLists.txt create mode 100644 src/modules/boat/CMakeLists.txt create mode 100644 src/modules/boat/Kconfig create mode 100644 src/modules/boat/module.yaml diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 34b255e255..02889c0ae0 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -72,8 +72,6 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL) px4_add_romfs_files( rc.rover_apps rc.rover_defaults - - rc.boat_defaults # hack ) endif() @@ -84,6 +82,13 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) ) endif() +if(CONFIG_MODULES_BOAT) + px4_add_romfs_files( + rc.boat_apps + rc.boat_defaults + ) +endif() + if(CONFIG_MODULES_UUV_ATT_CONTROL) px4_add_romfs_files( rc.uuv_apps diff --git a/ROMFS/px4fmu_common/init.d/airframes/70001_boat b/ROMFS/px4fmu_common/init.d/airframes/70001_boat new file mode 100644 index 0000000000..64e21b22bc --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/70001_boat @@ -0,0 +1,51 @@ +#!/bin/sh +# +# @name RC Boat +# +# @type Boat +# @class Boat +# +# @output Servo1 Steering +# @output Motor1 Throttle + +. ${R}etc/init.d/rc.boat_defaults + +# Failsafe +param set-default NAV_RCL_ACT 6 # Disarm on stick input loss +param set-default NAV_DLL_ACT 6 # Disarm on ground station loss +param set-default COM_DL_LOSS_T 5 # timeout for ground station connection +param set-default COM_RC_LOSS_T 10 # timeout for stick input +param set-default COM_RCL_EXCEPT 1 # Continue mission without stick input + +param set-default COM_ARM_WO_GPS 1 # Disable GPS prearm check +param set-default COM_DISARM_LAND -1 # Don't disarm automatically +param set-default COM_DISARM_PRFLT -1 + +# Battery setup +# param set-default BAT1_N_CELLS 1 +# param set-default BAT1_V_EMPTY 10 +# param set-default BAT1_V_CHARGED 18 + +# Estimation +param set-default EKF2_MULTI_IMU 0 # Disable multi-IMU +param set-default SENS_IMU_MODE 1 +param set-default CAL_MAG0_PRIO 0 # Disable internal Magnetometer +param set-default MBE_ENABLE 0 # Disable online mag calibration when disarmed +param set-default EKF2_GPS_CTRL 15 # Enable GPS heading fusion +param set-default SENS_GPS_MASK 0 # Disable multi-GPS blending + +# Estimation workarounds, should be double-checked if necessary +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_SYNT_MAG_Z 1 # Why? Who said magnetometer Z is not usable? + +param set-default EKF2_GPS_V_NOISE 0.751 # Lower certainty for GPS +param set-default EKF2_GPS_POS_X 2.5 # GPS position, accurate? +param set-default EKF2_REQ_EPV 10 # Accept GPS easier +param set-default EKF2_REQ_HDRIFT 1 +param set-default EKF2_REQ_SACC 2 +param set-default EKF2_REQ_VDRIFT 20 + +param set-default EKF2_MAG_CHK_STR 0.5 # Accept mag easier +param set-default COM_ARM_MAG_STR 0 # Disable magnetic field strength prearm check +param set-default COM_ARM_MAG_ANG 360 # Disable internal/external magnetometer consistency check +param set-default EKF2_ABL_LIM 1 # Higher accelerometer bias limit, was sometimes above the threshold, GPS reporting 0 altitude issue? diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_apps b/ROMFS/px4fmu_common/init.d/rc.boat_apps new file mode 100644 index 0000000000..b5b11cb590 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.boat_apps @@ -0,0 +1,11 @@ +#!/bin/sh +# Standard apps for a differential drive rover. + +# Start the attitude and position estimator. +ekf2 start & + +# Start rover differential drive controller. +boat start + +# Start Land Detector. +land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_defaults b/ROMFS/px4fmu_common/init.d/rc.boat_defaults index dbce64221c..84046da258 100644 --- a/ROMFS/px4fmu_common/init.d/rc.boat_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.boat_defaults @@ -5,7 +5,7 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -set VEHICLE_TYPE rover +set VEHICLE_TYPE boat # MAV_TYPE_SURFACE_BOAT 11 param set-default MAV_TYPE 11 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 42b9c7e876..b1ec6e70ce 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -32,6 +32,15 @@ then . ${R}etc/init.d/rc.rover_apps fi +# +# Boat setup. +# +if [ $VEHICLE_TYPE = boat ] +then + # Start standard Boat apps. + . ${R}etc/init.d/rc.boat_apps +fi + # # Differential Rover setup. # diff --git a/boards/px4/fmu-v5x/boat.px4board b/boards/px4/fmu-v5x/boat.px4board new file mode 100644 index 0000000000..64365e9bcc --- /dev/null +++ b/boards/px4/fmu-v5x/boat.px4board @@ -0,0 +1,17 @@ +CONFIG_DRIVERS_IRLOCK=n +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_BOAT=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +# CONFIG_EKF2_WIND is not set diff --git a/boards/px4/fmu-v6x/boat.px4board b/boards/px4/fmu-v6x/boat.px4board new file mode 100644 index 0000000000..64365e9bcc --- /dev/null +++ b/boards/px4/fmu-v6x/boat.px4board @@ -0,0 +1,17 @@ +CONFIG_DRIVERS_IRLOCK=n +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_BOAT=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +# CONFIG_EKF2_WIND is not set diff --git a/msg/BoatSetpoint.msg b/msg/BoatSetpoint.msg new file mode 100644 index 0000000000..7d33a039c1 --- /dev/null +++ b/msg/BoatSetpoint.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32 speed # [m/s] collective roll-off speed in body x-axis +bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward +float32 yaw_rate # [rad] yaw_rate +bool closed_loop_yaw_rate_control # true if rudder angle is controlled using encoder feedback, false if direct feed-forward + +# TOPICS boat_setpoint boat_control_output diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c53402bd0f..0b8e7a27d2 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -53,6 +53,7 @@ set(msg_files ArmingCheckRequest.msg AutotuneAttitudeControlStatus.msg BatteryStatus.msg + BoatSetpoint.msg Buffer128.msg ButtonEvent.msg CameraCapture.msg diff --git a/src/modules/boat/Boat.cpp b/src/modules/boat/Boat.cpp new file mode 100644 index 0000000000..2882bd67ad --- /dev/null +++ b/src/modules/boat/Boat.cpp @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 "Boat.hpp" + +Boat::Boat() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); +} + +bool Boat::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void Boat::updateParams() +{ + ModuleParams::updateParams(); + + // _boat_kinematics.setWheelBase(_param_rdd_wheel_base.get()); + + _max_speed = _param_bt_spd_max.get(); + _boat_guidance.setMaxSpeed(_max_speed); + _boat_kinematics.setMaxSpeed(_max_speed); + printf("max speed: %f\n", (double)_max_speed); + + _max_angular_velocity = _param_bt_ang_max.get(); + _boat_guidance.setMaxAngularVelocity(_max_angular_velocity); + _boat_kinematics.setMaxAngularVelocity(_max_angular_velocity); + printf("max angular velocity: %f\n", (double)_max_angular_velocity); +} + +void Boat::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + hrt_abstime now = hrt_absolute_time(); + const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f; + _time_stamp_last = now; + + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + } + + if (_vehicle_control_mode_sub.updated()) { + vehicle_control_mode_s vehicle_control_mode{}; + + if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { + _manual_driving = vehicle_control_mode.flag_control_manual_enabled; + _mission_driving = vehicle_control_mode.flag_control_auto_enabled; + } + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status{}; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s); + _boat_kinematics.setArmed(spooled_up); + _acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO); + } + } + + if (_manual_driving) { + // Manual mode + // directly produce setpoints from the manual control setpoint (joystick) + if (_manual_control_setpoint_sub.updated()) { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { + boat_setpoint_s setpoint{}; + setpoint.speed = ((manual_control_setpoint.throttle + 1.f) * 0.5f) * math::max(0.f, _param_bt_spd_scale.get()); + setpoint.yaw_rate = manual_control_setpoint.roll * _param_bt_ang_vel_scale.get(); + + // if acro mode, we activate the yaw rate control + if (_acro_driving) { + setpoint.closed_loop_speed_control = false; + setpoint.closed_loop_yaw_rate_control = true; + + } else { + setpoint.closed_loop_speed_control = false; + setpoint.closed_loop_yaw_rate_control = false; + } + + + setpoint.timestamp = now; + _boat_setpoint_pub.publish(setpoint); + } + } + + } else if (_mission_driving) { + // Mission mode + // directly receive setpoints from the guidance library + printf("I'm in mission mode\n"); + _boat_guidance.computeGuidance( + _boat_control.getVehicleYaw(), + _boat_control.getLocalPosition(), + dt + ); + } + + _boat_control.control(dt); + _boat_kinematics.allocate(); +} + +int Boat::task_spawn(int argc, char *argv[]) +{ + Boat *instance = new Boat(); + + 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 Boat::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int Boat::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Boat Drive controller. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("boat", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int boat_main(int argc, char *argv[]) +{ + return Boat::main(argc, argv); +} diff --git a/src/modules/boat/Boat.hpp b/src/modules/boat/Boat.hpp new file mode 100644 index 0000000000..cf206f5266 --- /dev/null +++ b/src/modules/boat/Boat.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "BoatControl/BoatControl.hpp" +#include "BoatGuidance/BoatGuidance.hpp" +#include "BoatKinematics/BoatKinematics.hpp" + +using namespace time_literals; + +class Boat : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + Boat(); + ~Boat() 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; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Publication _boat_setpoint_pub{ORB_ID(boat_setpoint)}; + + bool _manual_driving = false; + bool _mission_driving = false; + bool _acro_driving = false; + hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */ + + BoatGuidance _boat_guidance{this}; + BoatControl _boat_control{this}; + BoatKinematics _boat_kinematics{this}; + + float _max_speed{0.f}; + float _max_angular_velocity{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_com_spoolup_time, + (ParamFloat) _param_bt_ang_max, + (ParamFloat) _param_bt_spd_max, + (ParamFloat) _param_bt_ang_vel_scale, + (ParamFloat) _param_bt_spd_scale + ) +}; diff --git a/src/modules/boat/BoatControl/BoatControl.cpp b/src/modules/boat/BoatControl/BoatControl.cpp new file mode 100644 index 0000000000..ee2569cb6f --- /dev/null +++ b/src/modules/boat/BoatControl/BoatControl.cpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 "BoatControl.hpp" + +using namespace matrix; + +BoatControl::BoatControl(ModuleParams *parent) : ModuleParams(parent) +{ + pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +void BoatControl::updateParams() +{ + ModuleParams::updateParams(); + + pid_set_parameters(&_pid_angular_velocity, + _param_bt_ang_p.get(), + _param_bt_ang_i.get(), + 0, + _param_bt_ang_imax.get(), + _param_bt_ang_outlim.get()); + + pid_set_parameters(&_pid_speed, + _param_bt_spd_p.get(), + _param_bt_spd_i.get(), + 0, + _param_bt_spd_imax.get(), + _param_bt_spd_outlim.get()); +} + +void BoatControl::control(float dt) +{ + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + + if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) { + _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; + } + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + _vehicle_attitude_quaternion = 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{}; + + if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { + _vehicle_local_position = vehicle_local_position; + Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_forward_speed = velocity_in_body_frame(0); + } + } + + _boat_setpoint_sub.update(&_boat_setpoint); + + // PID to reach setpoint using control_output + boat_setpoint_s boat_control_output = _boat_setpoint; + + if (_boat_setpoint.closed_loop_speed_control) { + boat_control_output.speed += + pid_calculate(&_pid_speed, _boat_setpoint.speed, _vehicle_forward_speed, 0, dt); + } + + if (_boat_setpoint.closed_loop_yaw_rate_control) { + boat_control_output.yaw_rate += + pid_calculate(&_pid_angular_velocity, _boat_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt); + } + + boat_control_output.timestamp = hrt_absolute_time(); + _boat_control_output_pub.publish(boat_control_output); +} diff --git a/src/modules/boat/BoatControl/BoatControl.hpp b/src/modules/boat/BoatControl/BoatControl.hpp new file mode 100644 index 0000000000..be8a6e294c --- /dev/null +++ b/src/modules/boat/BoatControl/BoatControl.hpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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. + * + ****************************************************************************/ + +/** + * @file BoatControl.hpp + * + * Controller for heading rate and forward speed. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +class BoatControl : public ModuleParams +{ +public: + BoatControl(ModuleParams *parent); + ~BoatControl() = default; + + void control(float dt); + float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; } + float getVehicleYaw() const { return _vehicle_yaw; } + vehicle_local_position_s getLocalPosition() const { return _vehicle_local_position; } + +protected: + void updateParams() override; + +private: + uORB::Subscription _boat_setpoint_sub{ORB_ID(boat_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + + uORB::Publication _boat_control_output_pub{ORB_ID(boat_control_output)}; + + boat_setpoint_s _boat_setpoint{}; + + matrix::Quatf _vehicle_attitude_quaternion{}; + float _vehicle_yaw{0.f}; + + + // States + float _vehicle_body_yaw_rate{0.f}; + float _vehicle_forward_speed{0.f}; + Vector2f _vehicle_speed{0.f, 0.f}; + vehicle_local_position_s _vehicle_local_position{}; + + PID_t _pid_angular_velocity; + PID_t _pid_speed; + + DEFINE_PARAMETERS( + (ParamFloat) _param_bt_spd_p, + (ParamFloat) _param_bt_spd_i, + (ParamFloat) _param_bt_spd_imax, + (ParamFloat) _param_bt_spd_outlim, + (ParamFloat) _param_bt_ang_p, + (ParamFloat) _param_bt_ang_i, + (ParamFloat) _param_bt_ang_imax, + (ParamFloat) _param_bt_ang_outlim + ) +}; diff --git a/src/modules/boat/BoatControl/CMakeLists.txt b/src/modules/boat/BoatControl/CMakeLists.txt new file mode 100644 index 0000000000..9023822028 --- /dev/null +++ b/src/modules/boat/BoatControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2024 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(BoatControl + BoatControl.cpp +) + +target_link_libraries(BoatControl PUBLIC pid) +target_include_directories(BoatControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/boat/BoatGuidance/BoatGuidance.cpp b/src/modules/boat/BoatGuidance/BoatGuidance.cpp new file mode 100644 index 0000000000..cbd7fbdbdf --- /dev/null +++ b/src/modules/boat/BoatGuidance/BoatGuidance.cpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 "BoatGuidance.hpp" + +#include + +using namespace matrix; + +BoatGuidance::BoatGuidance(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + + _currentState = GuidanceState::DRIVING; +} + +void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_local_position, + float dt) +{ + if (_position_setpoint_triplet_sub.updated()) { + _position_setpoint_triplet_sub.copy(&_position_setpoint_triplet); + } + + if (_vehicle_global_position_sub.updated()) { + _vehicle_global_position_sub.copy(&_vehicle_global_position); + } + + const matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon); + const matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon); + const matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon); + const matrix::Vector2d previous_waypoint(_position_setpoint_triplet.previous.lat, + _position_setpoint_triplet.previous.lon); + + if (!_global_local_proj_ref.isInitialized() + || (_global_local_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.timestamp)) { + _global_local_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.timestamp); + } + + const Vector2f current_waypoint_local_position = _global_local_proj_ref.project(current_waypoint(0), + current_waypoint(1)); + const Vector2f previous_waypoint_local_position = _global_local_proj_ref.project(previous_waypoint(0), + previous_waypoint(1)); + const Vector2f local_position = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + const Vector2f local_velocity = Vector2f(vehicle_local_position.vx, vehicle_local_position.vy); + + const float distance_to_next_wp = get_distance_to_next_waypoint(local_position(0), local_position(1), + current_waypoint_local_position(0), + current_waypoint_local_position(1)); + + float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), next_waypoint(0), + next_waypoint(1)); + float heading_error = matrix::wrap_pi(desired_heading - yaw); + + if (_current_waypoint != current_waypoint) { + _currentState = GuidanceState::DRIVING; + } + + // Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly. + if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) { + _currentState = GuidanceState::GOAL_REACHED; + } + + float speed_interpolation = 0.f; + + if (PX4_ISFINITE(heading_error)) { + speed_interpolation = math::interpolate(abs(heading_error), _param_bt_min_heading_error.get() * M_PI_F / 180.f, + _param_bt_max_heading_error.get() * M_PI_F / 180.f, _param_bt_spd_cruise.get(), + _param_bt_spd_min.get()); + } + + float desired_speed = math::constrain(speed_interpolation, _param_bt_spd_min.get(), _max_speed); + + switch (_currentState) { + case GuidanceState::DRIVING: { + + if (PX4_ISFINITE(previous_waypoint(0)) && PX4_ISFINITE(previous_waypoint(1))) { + _l1_guidance.navigate_waypoints(previous_waypoint_local_position, current_waypoint_local_position, local_position, + local_velocity); + + } else { + _previous_local_position = local_position; + _currentState = GuidanceState::DRIVING_TO_POINT; + } + + break; + } + + case GuidanceState::DRIVING_TO_POINT: + + _l1_guidance.navigate_waypoints(_previous_local_position, current_waypoint_local_position, local_position, + local_velocity); + desired_speed = _param_bt_spd_cruise.get(); + break; + + case GuidanceState::GOAL_REACHED: + // temporary till I find a better way to stop the vehicle + desired_speed = 0.f; + heading_error = 0.f; + _desired_angular_velocity = 0.f; + break; + } + + _desired_angular_velocity = math::constrain(_l1_guidance.nav_lateral_acceleration_demand(), -_max_angular_velocity, + _max_angular_velocity); + + boat_setpoint_s output{}; + output.speed = math::constrain(desired_speed, -_max_speed, _max_speed); + output.yaw_rate = math::constrain(_desired_angular_velocity, -_max_angular_velocity, _max_angular_velocity); + output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true; + output.timestamp = hrt_absolute_time(); + + _boat_setpoint_pub.publish(output); + + _current_waypoint = current_waypoint; +} + +void BoatGuidance::updateParams() +{ + ModuleParams::updateParams(); + + _l1_guidance.set_l1_damping(_param_bt_l1_damping.get()); + _l1_guidance.set_l1_period(_param_bt_l1_period.get()); +} diff --git a/src/modules/boat/BoatGuidance/BoatGuidance.hpp b/src/modules/boat/BoatGuidance/BoatGuidance.hpp new file mode 100644 index 0000000000..3833e19da4 --- /dev/null +++ b/src/modules/boat/BoatGuidance/BoatGuidance.hpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace matrix; +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + DRIVING, ///< The vehicle is currently driving straight. + DRIVING_TO_POINT, ///< The vehicle is currently driving to the next waypoint. + GOAL_REACHED ///< The vehicle has reached its goal. +}; + +/** + * @brief Class for boat drive guidance. + */ +class BoatGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for BoatGuidance. + * @param parent The parent ModuleParams object. + */ + BoatGuidance(ModuleParams *parent); + ~BoatGuidance() = default; + + /** + * @brief Compute guidance for the vehicle. + * @param global_pos The global position of the vehicle in degrees. + * @param current_waypoint The current waypoint the vehicle is heading towards in degrees. + * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees. + * @param vehicle_yaw The yaw orientation of the vehicle in radians. + * @param body_velocity The velocity of the vehicle in m/s. + * @param angular_velocity The angular velocity of the vehicle in rad/s. + * @param dt The time step in seconds. + */ + void computeGuidance(float yaw, vehicle_local_position_s vehicle_local_position, float dt); + + /** + * @brief Set the maximum speed for the vehicle. + * @param max_speed The maximum speed in m/s. + * @return The set maximum speed in m/s. + */ + float setMaxSpeed(float max_speed) { return _max_speed = max_speed; } + + + /** + * @brief Set the maximum angular velocity for the vehicle. + * @param max_angular_velocity The maximum angular velocity in rad/s. + * @return The set maximum angular velocity in rad/s. + */ + float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; } + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + + uORB::Publication _boat_setpoint_pub{ORB_ID(boat_setpoint)}; + + position_setpoint_triplet_s _position_setpoint_triplet{}; + vehicle_global_position_s _vehicle_global_position{}; + + GuidanceState _currentState; + + float _desired_angular_velocity; + + float _max_speed; + float _max_angular_velocity; + + matrix::Vector2d _current_waypoint; + + VelocitySmoothing _forwards_velocity_smoothing; + PositionSmoothing _position_smoothing; + + ECL_L1_Pos_Controller _l1_guidance; + + Vector2f _previous_local_position{}; + + MapProjection _global_local_proj_ref{}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_nav_acc_rad, + + (ParamFloat) _param_bt_l1_period, + (ParamFloat) _param_bt_l1_damping, + + (ParamFloat) _param_bt_max_heading_error, + (ParamFloat) _param_bt_min_heading_error, + + (ParamFloat) _param_bt_spd_max, + (ParamFloat) _param_bt_spd_min, + + (ParamFloat) _param_bt_spd_cruise, + (ParamFloat) _param_nav_loiter_rad + ) +}; diff --git a/src/modules/boat/BoatGuidance/CMakeLists.txt b/src/modules/boat/BoatGuidance/CMakeLists.txt new file mode 100644 index 0000000000..d5957c39ca --- /dev/null +++ b/src/modules/boat/BoatGuidance/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2023-2024 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(BoatGuidance + BoatGuidance.cpp +) + +target_include_directories(BoatGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/boat/BoatKinematics/BoatKinematics.cpp b/src/modules/boat/BoatKinematics/BoatKinematics.cpp new file mode 100644 index 0000000000..6de9147d14 --- /dev/null +++ b/src/modules/boat/BoatKinematics/BoatKinematics.cpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (C) 2023-2024 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 "BoatKinematics.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +BoatKinematics::BoatKinematics(ModuleParams *parent) : ModuleParams(parent) +{} + +void BoatKinematics::allocate() +{ + hrt_abstime now = hrt_absolute_time(); + + if (_boat_control_output_sub.updated()) { + _boat_control_output_sub.copy(&_boat_control_output); + } + + const bool setpoint_timeout = (_boat_control_output.timestamp + 100_ms) < now; + + Vector2f boat_output = + computeInverseKinematics(_boat_control_output.speed, _boat_control_output.yaw_rate); + + if (!_armed || setpoint_timeout) { + boat_output = {}; // stop + } + + boat_output = matrix::constrain(boat_output, -1.f, 1.f); + + actuator_motors_s actuator_motors{}; + actuator_motors.control[0] = boat_output(0); + actuator_motors.control[1] = boat_output(0); + actuator_motors.timestamp = now; + _actuator_motors_pub.publish(actuator_motors); + + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = boat_output(1); + actuator_servos.control[1] = boat_output(1); + actuator_servos.timestamp = now; + _actuator_servos_pub.publish(actuator_servos); +} + +matrix::Vector2f BoatKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const +{ + if (_max_speed < FLT_EPSILON) { + return Vector2f(); + } + + linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed); + yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity); + + float throttle = linear_velocity_x / _max_speed; + float rudder_angle = yaw_rate / _max_angular_velocity; + + return Vector2f(throttle, rudder_angle); +} diff --git a/src/modules/boat/BoatKinematics/BoatKinematics.hpp b/src/modules/boat/BoatKinematics/BoatKinematics.hpp new file mode 100644 index 0000000000..f12c46f928 --- /dev/null +++ b/src/modules/boat/BoatKinematics/BoatKinematics.hpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 + +#include +#include +#include +#include +#include +#include +#include + + +/** + * @brief Boat Drive Kinematics class for computing the kinematics of a boat drive robot. + * + * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics + * given linear velocity and yaw rate. + */ +class BoatKinematics : public ModuleParams +{ +public: + BoatKinematics(ModuleParams *parent); + ~BoatKinematics() = default; + + /** + * @brief Sets the wheel base of the robot. + * + * @param wheel_base The distance between the centers of the wheels. + */ + void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; }; + + /** + * @brief Sets the maximum speed of the robot. + * + * @param max_speed The maximum speed of the robot. + */ + void setMaxSpeed(const float max_speed) { _max_speed = max_speed; }; + + /** + * @brief Sets the maximum angular speed of the robot. + * + * @param max_angular_speed The maximum angular speed of the robot. + */ + void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; }; + + void setArmed(const bool armed) { _armed = armed; }; + + void allocate(); + + /** + * @brief Computes the inverse kinematics for boat drive. + * + * @param linear_velocity_x Linear velocity along the x-axis. + * @param yaw_rate Yaw rate of the robot. + * @return matrix::Vector2f Motor velocities for the right and left motors. + */ + matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const; + +private: + uORB::Subscription _boat_control_output_sub{ORB_ID(boat_control_output)}; + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::PublicationMulti _actuator_servos_pub{ORB_ID(actuator_servos)}; + + boat_setpoint_s _boat_control_output{}; + bool _armed = false; + + float _wheel_base{0.f}; + float _max_speed{0.f}; + float _max_angular_velocity{0.f}; + + DEFINE_PARAMETERS( + (ParamInt) _param_r_rev + ) +}; diff --git a/src/modules/boat/BoatKinematics/CMakeLists.txt b/src/modules/boat/BoatKinematics/CMakeLists.txt new file mode 100644 index 0000000000..1e0cdac8b0 --- /dev/null +++ b/src/modules/boat/BoatKinematics/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2023-2024 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(BoatKinematics + BoatKinematics.cpp +) + +target_include_directories(BoatKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_functional_gtest(SRC BoatKinematicsTest.cpp LINKLIBS BoatKinematics) diff --git a/src/modules/boat/CMakeLists.txt b/src/modules/boat/CMakeLists.txt new file mode 100644 index 0000000000..cbfefd02ca --- /dev/null +++ b/src/modules/boat/CMakeLists.txt @@ -0,0 +1,53 @@ +############################################################################ +# +# Copyright (c) 2023-2024 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(BoatControl) +add_subdirectory(BoatGuidance) +add_subdirectory(BoatKinematics) + +px4_add_module( + MODULE modules__boat + MAIN boat + SRCS + Boat.cpp + Boat.hpp + DEPENDS + BoatControl + BoatGuidance + BoatKinematics + px4_work_queue + l1 + # modules__control_allocator # for parameter CA_R_REV + MODULE_CONFIG + module.yaml +) diff --git a/src/modules/boat/Kconfig b/src/modules/boat/Kconfig new file mode 100644 index 0000000000..e4ec6b8615 --- /dev/null +++ b/src/modules/boat/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_BOAT + bool "boat" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Enable support for control of boats diff --git a/src/modules/boat/module.yaml b/src/modules/boat/module.yaml new file mode 100644 index 0000000000..a7ec26aac0 --- /dev/null +++ b/src/modules/boat/module.yaml @@ -0,0 +1,201 @@ +module_name: Boat Drive + +parameters: + - group: Boat Drive + definitions: + BT_SPD_SCALE: + description: + short: Manual speed scale + type: float + min: 0 + max: 1 + increment: 0.01 + decimal: 2 + default: 1 + BT_ANG_VEL_SCALE: + description: + short: Manual angular velocity scale + type: float + min: 0 + max: 1 + increment: 0.01 + decimal: 2 + default: 1 + BT_LACC_LIM: + description: + short: Lateral acceleration limit + type: float + unit: m/s^2 + min: 0.0 + max: 6.0 + decimal: 1 + default: 2.0 + BT_L1_PERIOD: + description: + short: L1 period + type: float + unit: s + min: 0.5 + max: 50.0 + decimal: 1 + default: 25.0 + BT_L1_DAMPING: + description: + short: L1 damping + type: float + min: 0.6 + max: 0.9 + decimal: 2 + default: 0.9 + BT_SPD_CRUISE: + description: + short: Default cruise speed + type: float + unit: m/s + min: 0.0 + max: 50.0 + decimal: 1 + default: 7.0 + BT_SPD_MAX: + description: + short: Maximum speed + type: float + unit: m/s + min: 1.0 + max: 50.0 + decimal: 1 + default: 17.0 + BT_SPD_MIN: + description: + short: Minimum speed + type: float + unit: m/s + min: 0.0 + max: 50.0 + decimal: 1 + default: 2.0 + BT_SPD_P: + description: + short: Speed controller proportional gain + type: float + min: 0.0 + max: 2.0 + decimal: 2 + default: 2.0 + BT_SPD_I: + description: + short: Speed controller integral gain + type: float + min: 0.0 + max: 20.0 + decimal: 2 + default: 1.0 + BT_SPD_IMAX: + description: + short: Speed integral maximum value + type: float + unit: m/s + min: 0.0 + max: 20.0 + decimal: 1 + default: 20.0 + BT_SPD_OUTLIM: + description: + short: Speed PI controller output limit + type: float + unit: m/s + min: 0.0 + max: 200.0 + decimal: 1 + default: 50.0 + BT_ANG_MAX: + description: + short: Maximum angular velocity + type: float + unit: rad/s + min: 0.0 + max: 20.0 + decimal: 1 + default: 1.0 + BT_ANG_P: + description: + short: Angular velocity controller proportional gain + type: float + min: 0.0 + max: 2.0 + decimal: 2 + default: 0.2 + BT_ANG_I: + description: + short: Angular velocity controller integral gain + type: float + min: 0.0 + max: 20.0 + decimal: 2 + default: 0.0 + BT_ANG_IMAX: + description: + short: Angular velocity controller integral maximum value + type: float + unit: m/s + min: 0.0 + max: 20.0 + decimal: 1 + default: 20.0 + BT_ANG_OUTLIM: + description: + short: Angular velocity controller PI controller output limit + type: float + unit: m/s + min: 0.0 + max: 200.0 + decimal: 1 + default: 50.0 + BT_MAX_HERR: + description: + short: Max heading error for slowdown + type: float + min: 0.0 + decimal: 2 + default: 90.0 + BT_MIN_HERR: + description: + short: Min heading error for full speed + type: float + min: 0.0 + decimal: 2 + default: 30.0 + BT_MISSION_THR: + description: + short: Mission throttle setpoint + type: float + min: -1.0 + decimal: 2 + default: 0.8 + BT_MIN_THR: + description: + short: Mission minimum slowdown parameter + type: float + min: 0.0 + decimal: 2 + default: 0.5 + BT_MAX_JERK: + description: + short: Maximum jerk + type: float + unit: m/s^3 + min: 0.0 + max: 10.0 + decimal: 1 + default: 5.0 + BT_MAX_ACCEL: + description: + short: Maximum acceleration + long: Maximum acceleration is used to limit the acceleration of the rover + type: float + unit: m/s^2 + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.5