From 0ad8bcbdfe5ebec011aa501c208fbf0c60d1ada9 Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Sun, 21 Jan 2024 22:25:54 +0100 Subject: [PATCH 01/12] adding the boat controller module + reverse changes on the differential drive control allocation --- boards/px4/sitl/default.px4board | 1 + src/modules/boat_pos_control/CMakeLists.txt | 44 ++++ src/modules/boat_pos_control/Kconfig | 5 + .../boat_pos_control/boat_pos_control.cpp | 220 ++++++++++++++++++ .../boat_pos_control/boat_pos_control.hpp | 124 ++++++++++ src/modules/boat_pos_control/module.yaml | 15 ++ ...ActuatorEffectivenessRoverDifferential.cpp | 59 +++++ ...ActuatorEffectivenessRoverDifferential.hpp | 53 +++++ .../ActuatorEffectiveness/CMakeLists.txt | 2 + .../control_allocator/ControlAllocator.cpp | 1 + .../control_allocator/ControlAllocator.hpp | 1 + 11 files changed, 525 insertions(+) create mode 100644 src/modules/boat_pos_control/CMakeLists.txt create mode 100644 src/modules/boat_pos_control/Kconfig create mode 100644 src/modules/boat_pos_control/boat_pos_control.cpp create mode 100644 src/modules/boat_pos_control/boat_pos_control.hpp create mode 100644 src/modules/boat_pos_control/module.yaml create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 008dc97a1298..060880f1a419 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -8,6 +8,7 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BOAT_POS_CONTROL=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y diff --git a/src/modules/boat_pos_control/CMakeLists.txt b/src/modules/boat_pos_control/CMakeLists.txt new file mode 100644 index 000000000000..9a8126609dc8 --- /dev/null +++ b/src/modules/boat_pos_control/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015 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_module( + MODULE modules__boat_pos_control + MAIN boat_pos_control + SRCS + boat_pos_control.cpp + boat_pos_control.hpp + DEPENDS + px4_work_queue + MODULE_CONFIG + module.yaml + ) diff --git a/src/modules/boat_pos_control/Kconfig b/src/modules/boat_pos_control/Kconfig new file mode 100644 index 000000000000..ab16187e7278 --- /dev/null +++ b/src/modules/boat_pos_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_BOAT_POS_CONTROL + bool "boat_pos_control" + default n + ---help--- + Enable support for boat position controller diff --git a/src/modules/boat_pos_control/boat_pos_control.cpp b/src/modules/boat_pos_control/boat_pos_control.cpp new file mode 100644 index 000000000000..7d34f8b684bc --- /dev/null +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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_pos_control.hpp" + +BoatPosControl::BoatPosControl() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ +} + +BoatPosControl::~BoatPosControl() +{ + +} + +bool BoatPosControl::init() +{ + //Run on fixed interval + ScheduleOnInterval(10_ms); // 2000 us interval, 200 Hz rate + + return true; +} + +void BoatPosControl::parameters_update() +{ + if (_parameter_update_sub.updated()){ + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); + } + pid_init(&_velocity_pid, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_set_parameters(&_velocity_pid, + _param_usv_speed_p.get(), // Proportional gain + 0, // Integral gain + 0, // Derivative gain + 2, // Integral limit + 200); // Output limit +} + +void BoatPosControl::vehicle_attitude_poll() +{ + if (_att_sub.updated()) { + _att_sub.copy(&_vehicle_att); + } +} + +void BoatPosControl::Run() +{ + float dt = 0.01; // Using non zero value to a avoid division by zero + + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + parameters_update(); + vehicle_attitude_poll(); + + vehicle_control_mode_s vehicle_control_mode; + + vehicle_thrust_setpoint_s v_thrust_sp{}; + v_thrust_sp.timestamp = hrt_absolute_time(); + v_thrust_sp.xyz[0] = 0.0f; + v_thrust_sp.xyz[1] = 0.0f; + v_thrust_sp.xyz[2] = 0.0f; + + vehicle_torque_setpoint_s v_torque_sp{}; + v_torque_sp.timestamp = hrt_absolute_time(); + v_torque_sp.xyz[0] = 0.f; + v_torque_sp.xyz[1] = 0.f; + + if (_vehicle_control_mode_sub.updated()) { + + + if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { + _armed = vehicle_control_mode.flag_armed; + _position_ctrl_ena = vehicle_control_mode.flag_control_position_enabled; // change this when more modes are supported + } + } + if (_armed && _position_ctrl_ena){ + if (_local_pos_sub.update(&_local_pos)) { + _manual_control_setpoint_sub.copy(&_manual_control_setpoint); + + // Velocity in body frame + const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); + const Vector3f vel = R_to_body * Vector3f(_local_pos.vx, _local_pos.vy, _local_pos.vz); + + // Speed control + float _thrust = pid_calculate(&_velocity_pid, _manual_control_setpoint.throttle*7.f, vel(0), 0, dt); + + v_thrust_sp.xyz[0] = _thrust; + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); + + + // yaw rate control + v_torque_sp.xyz[2] = 0.f; + _vehicle_torque_setpoint_pub.publish(v_torque_sp); + } + } + else if (_armed && vehicle_control_mode.flag_control_manual_enabled) { + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)){ + + v_thrust_sp.xyz[0] = _manual_control_setpoint.throttle; + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); + + v_torque_sp.xyz[2] = _manual_control_setpoint.roll; + _vehicle_torque_setpoint_pub.publish(v_torque_sp); + } + + } + else { + + v_thrust_sp.xyz[0] = 0.0f; + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); + + + + v_torque_sp.xyz[0] = 0.f; + v_torque_sp.xyz[1] = 0.f; + v_torque_sp.xyz[2] = 0.f; + _vehicle_torque_setpoint_pub.publish(v_torque_sp); + } + + +} + +int BoatPosControl::task_spawn(int argc, char *argv[]) +{ + BoatPosControl *instance = new BoatPosControl(); + + 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 BoatPosControl::print_status() +{ + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + return 0; +} + +int BoatPosControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int BoatPosControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +boat controller + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("boat_pos_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int boat_pos_control_main(int argc, char *argv[]) +{ + return BoatPosControl::main(argc, argv); +} diff --git a/src/modules/boat_pos_control/boat_pos_control.hpp b/src/modules/boat_pos_control/boat_pos_control.hpp new file mode 100644 index 000000000000..6737509b06c5 --- /dev/null +++ b/src/modules/boat_pos_control/boat_pos_control.hpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 +#include +#include +#include +#include +#include +#include +#include +#include + +using matrix::Dcmf; + +using namespace matrix; + +using namespace time_literals; + +class BoatPosControl : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + BoatPosControl(); + ~BoatPosControl() override; + + /** @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(); + + int print_status() override; + + +private: + void Run() override; + void parameters_update() ; + void vehicle_attitude_poll(); + // Subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + + + // Publications + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + + // Performance (perf) counters + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + + + PID_t _velocity_pid; ///< The PID controller for velocity. + vehicle_local_position_s _local_pos{}; /**< global vehicle position */ + manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ + vehicle_attitude_s _vehicle_att{}; + + bool _armed = false; + bool _position_ctrl_ena = false; + + DEFINE_PARAMETERS((ParamFloat) _param_usv_speed_p) + + + + +}; diff --git a/src/modules/boat_pos_control/module.yaml b/src/modules/boat_pos_control/module.yaml new file mode 100644 index 000000000000..c490919234ac --- /dev/null +++ b/src/modules/boat_pos_control/module.yaml @@ -0,0 +1,15 @@ +module_name: Boat Position Control + +parameters: + - group: Boat Position Control + definitions: + USV_SPEED_P: + description: + short: Speed proportional gain + type: float + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 1 + diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp new file mode 100644 index 000000000000..8e7c64baa851 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 "ActuatorEffectivenessRoverDifferential.hpp" +#include + +using namespace matrix; + +bool +ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, 0.5f}, Vector3f{0.5f, 0.f, 0.f}); + configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, -0.5f}, Vector3f{0.5f, 0.f, 0.f}); + _motors_mask = (1u << 0) | (1u << 1); + return true; +} + +void ActuatorEffectivenessRoverDifferential::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); +} + diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp new file mode 100644 index 000000000000..fed0a7ddcf8b --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 "ActuatorEffectiveness.hpp" + +class ActuatorEffectivenessRoverDifferential: public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessRoverDifferential() = default; + virtual ~ActuatorEffectivenessRoverDifferential() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + const char *name() const override { return "Rover (Differential)"; } +private: + uint32_t _motors_mask{}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 2406b81bf80b..994b566270f1 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -62,6 +62,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp + ActuatorEffectivenessRoverDifferential.hpp + ActuatorEffectivenessRoverDifferential.cpp ) target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 2ead22c57079..2fbc44a69f5e 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -239,6 +239,7 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::ROVER_DIFFERENTIAL: + tmp = new ActuatorEffectivenessRoverDifferential(); // differential_drive_control does allocation and publishes directly to actuator_motors topic break; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 303316f1ef52..25904f05515e 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include From 4883a6c8f4150b01e9d6a763a04599f0dd50eee2 Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Sat, 16 Mar 2024 19:46:43 +0100 Subject: [PATCH 02/12] Manual mode ready --- .../boat_pos_control/boat_pos_control.cpp | 64 ++++++++++++------- .../boat_pos_control/boat_pos_control.hpp | 13 +++- src/modules/boat_pos_control/module.yaml | 9 +++ 3 files changed, 62 insertions(+), 24 deletions(-) diff --git a/src/modules/boat_pos_control/boat_pos_control.cpp b/src/modules/boat_pos_control/boat_pos_control.cpp index 7d34f8b684bc..a4b62ff084c1 100644 --- a/src/modules/boat_pos_control/boat_pos_control.cpp +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -63,6 +63,7 @@ void BoatPosControl::parameters_update() // this class attributes need updating (and do so). updateParams(); } + // init velocity controller pid_init(&_velocity_pid, PID_MODE_DERIVATIV_NONE, 0.001f); pid_set_parameters(&_velocity_pid, _param_usv_speed_p.get(), // Proportional gain @@ -70,6 +71,15 @@ void BoatPosControl::parameters_update() 0, // Derivative gain 2, // Integral limit 200); // Output limit + + // init yaw rate controller + pid_init(&_yaw_rate_pid, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_set_parameters(&_yaw_rate_pid, + _param_usv_yaw_rate_p.get(), // Proportional gain + 0, // Integral gain + 0, // Derivative gain + 2, // Integral limit + 200); // Output limit } void BoatPosControl::vehicle_attitude_poll() @@ -83,6 +93,12 @@ void BoatPosControl::Run() { float dt = 0.01; // Using non zero value to a avoid division by zero + /* advertise debug value */ + struct debug_key_value_s dbg; + strncpy(dbg.key, "yaw", sizeof(dbg.key)); + dbg.value = 0.0f; + orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + if (should_exit()) { ScheduleClear(); exit_and_cleanup(); @@ -91,8 +107,6 @@ void BoatPosControl::Run() parameters_update(); vehicle_attitude_poll(); - vehicle_control_mode_s vehicle_control_mode; - vehicle_thrust_setpoint_s v_thrust_sp{}; v_thrust_sp.timestamp = hrt_absolute_time(); v_thrust_sp.xyz[0] = 0.0f; @@ -104,6 +118,12 @@ void BoatPosControl::Run() v_torque_sp.xyz[0] = 0.f; v_torque_sp.xyz[1] = 0.f; + const Quatf q{_vehicle_att.q}; + const float yaw = Eulerf(q).psi(); + + dbg.value = yaw; + orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + if (_vehicle_control_mode_sub.updated()) { @@ -122,41 +142,39 @@ void BoatPosControl::Run() // Speed control float _thrust = pid_calculate(&_velocity_pid, _manual_control_setpoint.throttle*7.f, vel(0), 0, dt); + _thrust = _thrust + 0.f; - v_thrust_sp.xyz[0] = _thrust; - _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); + // yaw rate control + yaw_setpoint += _manual_control_setpoint.roll*0.1f; + float _torque_sp = pid_calculate(&_yaw_rate_pid, yaw_setpoint, yaw, 0, dt); - // yaw rate control - v_torque_sp.xyz[2] = 0.f; + v_thrust_sp.xyz[0] = 0; + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); + + v_torque_sp.xyz[2] = -_torque_sp; _vehicle_torque_setpoint_pub.publish(v_torque_sp); } } else if (_armed && vehicle_control_mode.flag_control_manual_enabled) { - if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)){ + _manual_control_setpoint_sub.copy(&_manual_control_setpoint); + v_thrust_sp.xyz[0] = _manual_control_setpoint.throttle; + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); - v_thrust_sp.xyz[0] = _manual_control_setpoint.throttle; - _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); + v_torque_sp.xyz[2] = -_manual_control_setpoint.roll*0.1f; + _vehicle_torque_setpoint_pub.publish(v_torque_sp); - v_torque_sp.xyz[2] = _manual_control_setpoint.roll; - _vehicle_torque_setpoint_pub.publish(v_torque_sp); - } } else { - - v_thrust_sp.xyz[0] = 0.0f; - _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); - - - - v_torque_sp.xyz[0] = 0.f; - v_torque_sp.xyz[1] = 0.f; - v_torque_sp.xyz[2] = 0.f; - _vehicle_torque_setpoint_pub.publish(v_torque_sp); + v_thrust_sp.xyz[0] = 0.0f; + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); + v_torque_sp.xyz[0] = 0.f; + v_torque_sp.xyz[1] = 0.f; + v_torque_sp.xyz[2] = 0.f; + _vehicle_torque_setpoint_pub.publish(v_torque_sp); } - } int BoatPosControl::task_spawn(int argc, char *argv[]) diff --git a/src/modules/boat_pos_control/boat_pos_control.hpp b/src/modules/boat_pos_control/boat_pos_control.hpp index 6737509b06c5..74f0dbd9576a 100644 --- a/src/modules/boat_pos_control/boat_pos_control.hpp +++ b/src/modules/boat_pos_control/boat_pos_control.hpp @@ -61,6 +61,10 @@ #include #include +#include +#include +#include + using matrix::Dcmf; using namespace matrix; @@ -99,6 +103,8 @@ class BoatPosControl : public ModuleBase, public ModuleParams, p uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + + // Publications uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; @@ -109,14 +115,19 @@ class BoatPosControl : public ModuleBase, public ModuleParams, p PID_t _velocity_pid; ///< The PID controller for velocity. + PID_t _yaw_rate_pid; ///< The PID controller for yaw rate. vehicle_local_position_s _local_pos{}; /**< global vehicle position */ manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ vehicle_attitude_s _vehicle_att{}; + float yaw_setpoint; bool _armed = false; bool _position_ctrl_ena = false; + vehicle_control_mode_s vehicle_control_mode; - DEFINE_PARAMETERS((ParamFloat) _param_usv_speed_p) + DEFINE_PARAMETERS((ParamFloat) _param_usv_speed_p, + (ParamFloat) _param_usv_yaw_rate_p + ) diff --git a/src/modules/boat_pos_control/module.yaml b/src/modules/boat_pos_control/module.yaml index c490919234ac..41948b61dc13 100644 --- a/src/modules/boat_pos_control/module.yaml +++ b/src/modules/boat_pos_control/module.yaml @@ -12,4 +12,13 @@ parameters: increment: 0.001 decimal: 3 default: 1 + USV_YAW_RATE_P: + description: + short: Yaw rate proportional gain + type: float + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.1 From 9b43c723a47a2710b306b53bf173bf8778bb086b Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Sat, 16 Mar 2024 20:03:30 +0100 Subject: [PATCH 03/12] dummy commit --- ROMFS/px4fmu_common/init.d/rc.rover_differential_apps | 8 +++++++- src/modules/boat_pos_control/boat_pos_control.cpp | 6 +++--- src/modules/mavlink/mavlink_receiver.cpp | 3 ++- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps index 31c79ac9801e..f4821b2843fa 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps @@ -5,7 +5,13 @@ ekf2 start & # Start rover differential drive controller. -differential_drive_control start +#differential_drive_control start +control_allocator start + +# We use rover start script to test the controller +# Start boat position controller. +boat_pos_control start + # Start Land Detector. land_detector start rover diff --git a/src/modules/boat_pos_control/boat_pos_control.cpp b/src/modules/boat_pos_control/boat_pos_control.cpp index a4b62ff084c1..1b90863f8a04 100644 --- a/src/modules/boat_pos_control/boat_pos_control.cpp +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -145,11 +145,11 @@ void BoatPosControl::Run() _thrust = _thrust + 0.f; // yaw rate control - yaw_setpoint += _manual_control_setpoint.roll*0.1f; + //yaw_setpoint += _manual_control_setpoint.roll*0.1f; - float _torque_sp = pid_calculate(&_yaw_rate_pid, yaw_setpoint, yaw, 0, dt); + float _torque_sp = pid_calculate(&_yaw_rate_pid, 0.f, yaw, 0, dt); - v_thrust_sp.xyz[0] = 0; + v_thrust_sp.xyz[0] = _thrust; _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); v_torque_sp.xyz[2] = -_torque_sp; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6030d9b651ba..02b030f793c8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2087,7 +2087,8 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual_control_setpoint.pitch = mavlink_manual_control.x / 1000.f; manual_control_setpoint.roll = mavlink_manual_control.y / 1000.f; // For backwards compatibility at the moment interpret throttle in range [0,1000] - manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; + //manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; + manual_control_setpoint.throttle = mavlink_manual_control.z / 1000.f; manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f; manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time(); From 903dd2a6d6a25b93639079e2423323d256796cc9 Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Sun, 17 Mar 2024 21:09:37 +0100 Subject: [PATCH 04/12] Moving to the position setpoint --- .../boat_pos_control/boat_pos_control.cpp | 33 ++++++++++++++++--- .../boat_pos_control/boat_pos_control.hpp | 6 ++++ 2 files changed, 34 insertions(+), 5 deletions(-) diff --git a/src/modules/boat_pos_control/boat_pos_control.cpp b/src/modules/boat_pos_control/boat_pos_control.cpp index 1b90863f8a04..336f68352863 100644 --- a/src/modules/boat_pos_control/boat_pos_control.cpp +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -121,8 +121,7 @@ void BoatPosControl::Run() const Quatf q{_vehicle_att.q}; const float yaw = Eulerf(q).psi(); - dbg.value = yaw; - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + if (_vehicle_control_mode_sub.updated()) { @@ -132,6 +131,29 @@ void BoatPosControl::Run() _position_ctrl_ena = vehicle_control_mode.flag_control_position_enabled; // change this when more modes are supported } } + + 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); + } + + matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon); + matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon); + matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon); + + const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), + current_waypoint(0), + current_waypoint(1)); + + float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), + current_waypoint(1)); + + dbg.value = desired_heading; + orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + if (_armed && _position_ctrl_ena){ if (_local_pos_sub.update(&_local_pos)) { _manual_control_setpoint_sub.copy(&_manual_control_setpoint); @@ -141,13 +163,14 @@ void BoatPosControl::Run() const Vector3f vel = R_to_body * Vector3f(_local_pos.vx, _local_pos.vy, _local_pos.vz); // Speed control - float _thrust = pid_calculate(&_velocity_pid, _manual_control_setpoint.throttle*7.f, vel(0), 0, dt); - _thrust = _thrust + 0.f; + float _thrust = pid_calculate(&_velocity_pid, distance_to_next_wp, vel(0), 0, dt); + _thrust = math::constrain(_thrust, -1.0f, 1.0f); + //_thrust = _thrust + 0.f; // yaw rate control //yaw_setpoint += _manual_control_setpoint.roll*0.1f; - float _torque_sp = pid_calculate(&_yaw_rate_pid, 0.f, yaw, 0, dt); + float _torque_sp = pid_calculate(&_yaw_rate_pid, desired_heading, yaw, 0, dt); v_thrust_sp.xyz[0] = _thrust; _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); diff --git a/src/modules/boat_pos_control/boat_pos_control.hpp b/src/modules/boat_pos_control/boat_pos_control.hpp index 74f0dbd9576a..fc5bc3519cba 100644 --- a/src/modules/boat_pos_control/boat_pos_control.hpp +++ b/src/modules/boat_pos_control/boat_pos_control.hpp @@ -59,6 +59,8 @@ #include #include #include +#include +#include #include #include @@ -101,6 +103,8 @@ class BoatPosControl : public ModuleBase, public ModuleParams, p uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; @@ -117,6 +121,8 @@ class BoatPosControl : public ModuleBase, public ModuleParams, p PID_t _velocity_pid; ///< The PID controller for velocity. PID_t _yaw_rate_pid; ///< The PID controller for yaw rate. vehicle_local_position_s _local_pos{}; /**< global vehicle position */ + position_setpoint_triplet_s _position_setpoint_triplet{}; + vehicle_global_position_s _vehicle_global_position{}; manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ vehicle_attitude_s _vehicle_att{}; float yaw_setpoint; From c0fc0af28269de7b5b6ca256f02e2b083fb5062a Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Sat, 23 Mar 2024 22:52:56 +0100 Subject: [PATCH 05/12] Implementing the shortest path to reach the desired direction --- .../boat_pos_control/boat_pos_control.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/boat_pos_control/boat_pos_control.cpp b/src/modules/boat_pos_control/boat_pos_control.cpp index 336f68352863..1fb058230740 100644 --- a/src/modules/boat_pos_control/boat_pos_control.cpp +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -95,7 +95,7 @@ void BoatPosControl::Run() /* advertise debug value */ struct debug_key_value_s dbg; - strncpy(dbg.key, "yaw", sizeof(dbg.key)); + strncpy(dbg.key, "debug_val", sizeof(dbg.key)); dbg.value = 0.0f; orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); @@ -119,7 +119,7 @@ void BoatPosControl::Run() v_torque_sp.xyz[1] = 0.f; const Quatf q{_vehicle_att.q}; - const float yaw = Eulerf(q).psi(); + float yaw = Eulerf(q).psi(); @@ -151,8 +151,7 @@ void BoatPosControl::Run() float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), current_waypoint(1)); - dbg.value = desired_heading; - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + if (_armed && _position_ctrl_ena){ if (_local_pos_sub.update(&_local_pos)) { @@ -170,6 +169,15 @@ void BoatPosControl::Run() // yaw rate control //yaw_setpoint += _manual_control_setpoint.roll*0.1f; + // Adjust the setpoint to take the shortest path + float heading_error = desired_heading - yaw; + if (abs(heading_error)>M_PI_F){ + float new_heading_error = 2*M_PI_F-abs(heading_error); + desired_heading = -sign(heading_error)*new_heading_error+yaw; + } + dbg.value = desired_heading; + orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + float _torque_sp = pid_calculate(&_yaw_rate_pid, desired_heading, yaw, 0, dt); v_thrust_sp.xyz[0] = _thrust; From c1dd84f41a83c678eecb398dfd5beadd164fc9c5 Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Sun, 24 Mar 2024 20:09:23 +0100 Subject: [PATCH 06/12] position stabilization on the target --- .../boat_pos_control/boat_pos_control.cpp | 19 +++++++++++++++---- .../boat_pos_control/boat_pos_control.hpp | 7 +++---- src/modules/boat_pos_control/module.yaml | 9 +++++++++ 3 files changed, 27 insertions(+), 8 deletions(-) diff --git a/src/modules/boat_pos_control/boat_pos_control.cpp b/src/modules/boat_pos_control/boat_pos_control.cpp index 1fb058230740..58bd85b6f9b7 100644 --- a/src/modules/boat_pos_control/boat_pos_control.cpp +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -144,7 +144,7 @@ void BoatPosControl::Run() matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon); matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon); - const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), + float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), current_waypoint(1)); @@ -161,6 +161,13 @@ void BoatPosControl::Run() const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); const Vector3f vel = R_to_body * Vector3f(_local_pos.vx, _local_pos.vy, _local_pos.vz); + //once position reached + if (distance_to_next_wp<_param_usv_dist_epsi.get()){ + desired_heading = yaw; // keep the last direction + distance_to_next_wp = 0.0f; + + } + // Speed control float _thrust = pid_calculate(&_velocity_pid, distance_to_next_wp, vel(0), 0, dt); _thrust = math::constrain(_thrust, -1.0f, 1.0f); @@ -175,16 +182,20 @@ void BoatPosControl::Run() float new_heading_error = 2*M_PI_F-abs(heading_error); desired_heading = -sign(heading_error)*new_heading_error+yaw; } - dbg.value = desired_heading; + + + dbg.value = distance_to_next_wp; orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); float _torque_sp = pid_calculate(&_yaw_rate_pid, desired_heading, yaw, 0, dt); + _torque_sp = math::constrain(_torque_sp, -1.0f, 1.0f); v_thrust_sp.xyz[0] = _thrust; - _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); - v_torque_sp.xyz[2] = -_torque_sp; + + _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); _vehicle_torque_setpoint_pub.publish(v_torque_sp); + } } else if (_armed && vehicle_control_mode.flag_control_manual_enabled) { diff --git a/src/modules/boat_pos_control/boat_pos_control.hpp b/src/modules/boat_pos_control/boat_pos_control.hpp index fc5bc3519cba..198b47506ee4 100644 --- a/src/modules/boat_pos_control/boat_pos_control.hpp +++ b/src/modules/boat_pos_control/boat_pos_control.hpp @@ -97,6 +97,7 @@ class BoatPosControl : public ModuleBase, public ModuleParams, p void Run() override; void parameters_update() ; void vehicle_attitude_poll(); + // Subscriptions uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; @@ -106,9 +107,6 @@ class BoatPosControl : public ModuleBase, public ModuleParams, p uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - - - // Publications uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; @@ -132,7 +130,8 @@ class BoatPosControl : public ModuleBase, public ModuleParams, p vehicle_control_mode_s vehicle_control_mode; DEFINE_PARAMETERS((ParamFloat) _param_usv_speed_p, - (ParamFloat) _param_usv_yaw_rate_p + (ParamFloat) _param_usv_yaw_rate_p, + (ParamFloat) _param_usv_dist_epsi ) diff --git a/src/modules/boat_pos_control/module.yaml b/src/modules/boat_pos_control/module.yaml index 41948b61dc13..3f391f326b7f 100644 --- a/src/modules/boat_pos_control/module.yaml +++ b/src/modules/boat_pos_control/module.yaml @@ -21,4 +21,13 @@ parameters: increment: 0.001 decimal: 3 default: 0.1 + USV_DIST_EPSI: + description: + short: Accuracy of distance to target + type: float + min: 0.001 + max: 10 + increment: 0.001 + decimal: 3 + default: 0.01 From b2b4c9f7eb0bafe8a283444ff96a144099d570de Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Fri, 5 Apr 2024 14:00:51 +0200 Subject: [PATCH 07/12] Adding boat differential Airframe --- .../init.d/rc.boat_differential_defaults | 4 ++-- .../boat_pos_control/boat_pos_control.cpp | 17 ++++++++++++++--- ...> ActuatorEffectivenessBoatDifferential.cpp} | 6 +++--- ...> ActuatorEffectivenessBoatDifferential.hpp} | 8 ++++---- .../ActuatorEffectiveness/CMakeLists.txt | 4 ++-- .../control_allocator/ControlAllocator.cpp | 5 ++++- .../control_allocator/ControlAllocator.hpp | 3 ++- src/modules/control_allocator/module.yaml | 10 ++++++++++ 8 files changed, 41 insertions(+), 16 deletions(-) rename src/modules/control_allocator/ActuatorEffectiveness/{ActuatorEffectivenessRoverDifferential.cpp => ActuatorEffectivenessBoatDifferential.cpp} (90%) rename src/modules/control_allocator/ActuatorEffectiveness/{ActuatorEffectivenessRoverDifferential.hpp => ActuatorEffectivenessBoatDifferential.hpp} (89%) diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults b/ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults index 9529d5f9178a..e9613ef84a1f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults @@ -3,9 +3,9 @@ set VEHICLE_TYPE boat -param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER +param set-default MAV_TYPE 11 # Surface vessel -param set-default CA_AIRFRAME 6 # Rover (Differential) +param set-default CA_AIRFRAME 13 # Boat (Differential) param set-default CA_R_REV 3 # Right and left motors reversible param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying diff --git a/src/modules/boat_pos_control/boat_pos_control.cpp b/src/modules/boat_pos_control/boat_pos_control.cpp index 58bd85b6f9b7..b30c2a67bc10 100644 --- a/src/modules/boat_pos_control/boat_pos_control.cpp +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -168,9 +168,7 @@ void BoatPosControl::Run() } - // Speed control - float _thrust = pid_calculate(&_velocity_pid, distance_to_next_wp, vel(0), 0, dt); - _thrust = math::constrain(_thrust, -1.0f, 1.0f); + //_thrust = _thrust + 0.f; // yaw rate control @@ -187,6 +185,19 @@ void BoatPosControl::Run() dbg.value = distance_to_next_wp; orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + // Set the thrust to 0 when the heading error is too high + /*if (fabsf(heading_error)>=0.5f){ + distance_to_next_wp = 0; + }*/ + + + // Speed control + float _thrust = pid_calculate(&_velocity_pid, distance_to_next_wp, vel(0), 0, dt); + _thrust = _thrust * (1-(heading_error/M_PI_F)); + _thrust = math::constrain(_thrust, -1.0f, 0.9f); + + + float _torque_sp = pid_calculate(&_yaw_rate_pid, desired_heading, yaw, 0, dt); _torque_sp = math::constrain(_torque_sp, -1.0f, 1.0f); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessBoatDifferential.cpp similarity index 90% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp rename to src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessBoatDifferential.cpp index 8e7c64baa851..eac687021305 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessBoatDifferential.cpp @@ -31,13 +31,13 @@ * ****************************************************************************/ -#include "ActuatorEffectivenessRoverDifferential.hpp" +#include "ActuatorEffectivenessBoatDifferential.hpp" #include using namespace matrix; bool -ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration, +ActuatorEffectivenessBoatDifferential::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { @@ -50,7 +50,7 @@ ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &co return true; } -void ActuatorEffectivenessRoverDifferential::updateSetpoint(const matrix::Vector &control_sp, +void ActuatorEffectivenessBoatDifferential::updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, const matrix::Vector &actuator_max) { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessBoatDifferential.hpp similarity index 89% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp rename to src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessBoatDifferential.hpp index fed0a7ddcf8b..ede11f07994f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessBoatDifferential.hpp @@ -35,11 +35,11 @@ #include "ActuatorEffectiveness.hpp" -class ActuatorEffectivenessRoverDifferential: public ActuatorEffectiveness +class ActuatorEffectivenessBoatDifferential: public ActuatorEffectiveness { public: - ActuatorEffectivenessRoverDifferential() = default; - virtual ~ActuatorEffectivenessRoverDifferential() = default; + ActuatorEffectivenessBoatDifferential() = default; + virtual ~ActuatorEffectivenessBoatDifferential() = default; bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; @@ -47,7 +47,7 @@ class ActuatorEffectivenessRoverDifferential: public ActuatorEffectiveness ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, const matrix::Vector &actuator_max) override; - const char *name() const override { return "Rover (Differential)"; } + const char *name() const override { return "Boat (Differential)"; } private: uint32_t _motors_mask{}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 994b566270f1..e6c7d4569b9f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -62,8 +62,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp - ActuatorEffectivenessRoverDifferential.hpp - ActuatorEffectivenessRoverDifferential.cpp + ActuatorEffectivenessBoatDifferential.hpp + ActuatorEffectivenessBoatDifferential.cpp ) target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 2fbc44a69f5e..79bebb29e704 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -239,7 +239,6 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::ROVER_DIFFERENTIAL: - tmp = new ActuatorEffectivenessRoverDifferential(); // differential_drive_control does allocation and publishes directly to actuator_motors topic break; @@ -271,6 +270,10 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessHelicopterCoaxial(this); break; + case EffectivenessSource::BOAT_DIFFERENTIAL: + tmp = new ActuatorEffectivenessBoatDifferential(); + break; + default: PX4_ERR("Unknown airframe"); break; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 25904f05515e..436475fc83a3 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include #include #include @@ -159,6 +159,7 @@ class ControlAllocator : public ModuleBase, public ModuleParam HELICOPTER_TAIL_ESC = 10, HELICOPTER_TAIL_SERVO = 11, HELICOPTER_COAXIAL = 12, + BOAT_DIFFERENTIAL = 13 }; enum class FailureMode { diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 8683d7477d95..000309836f79 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -30,6 +30,7 @@ parameters: 10: Helicopter (tail ESC) 11: Helicopter (tail Servo) 12: Helicopter (Coaxial) + 13: Boat (Differential) default: 0 CA_METHOD: @@ -1140,3 +1141,12 @@ mixer: parameters: - label: 'Throttle spoolup time' name: COM_SPOOLUP_TIME + 13: # Boat (Differential) + title: 'Boat (Differential)' + actuators: + - actuator_type: 'motor' + instances: + - name: 'Right Motor' + position: [ 0, 1., 0 ] + - name: 'Left Motor' + position: [ 0, -1., 0 ] From 3a7d2f0637a6ff8dda99cd1be79fbd5d033f46d5 Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Sun, 7 Apr 2024 17:31:05 +0200 Subject: [PATCH 08/12] adjusting speed setpoint according to positioning step --- .../init.d/rc.boat_differential_defaults | 2 + .../boat_pos_control/boat_pos_control.cpp | 65 +++++++++++-------- .../boat_pos_control/boat_pos_control.hpp | 14 +++- src/modules/boat_pos_control/module.yaml | 47 +++++++++++++- 4 files changed, 100 insertions(+), 28 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults b/ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults index e9613ef84a1f..2674f2719443 100644 --- a/ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults @@ -8,4 +8,6 @@ param set-default MAV_TYPE 11 # Surface vessel param set-default CA_AIRFRAME 13 # Boat (Differential) param set-default CA_R_REV 3 # Right and left motors reversible +param set-default NAV_ACC_RAD 1.5 # mission acceptance radius + param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying diff --git a/src/modules/boat_pos_control/boat_pos_control.cpp b/src/modules/boat_pos_control/boat_pos_control.cpp index b30c2a67bc10..96178cbb022e 100644 --- a/src/modules/boat_pos_control/boat_pos_control.cpp +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -76,9 +76,9 @@ void BoatPosControl::parameters_update() pid_init(&_yaw_rate_pid, PID_MODE_DERIVATIV_NONE, 0.001f); pid_set_parameters(&_yaw_rate_pid, _param_usv_yaw_rate_p.get(), // Proportional gain - 0, // Integral gain + _param_usv_yaw_i.get(), // Integral gain 0, // Derivative gain - 2, // Integral limit + 1, // Integral limit 200); // Output limit } @@ -160,53 +160,66 @@ void BoatPosControl::Run() // Velocity in body frame const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); const Vector3f vel = R_to_body * Vector3f(_local_pos.vx, _local_pos.vy, _local_pos.vz); - - //once position reached - if (distance_to_next_wp<_param_usv_dist_epsi.get()){ - desired_heading = yaw; // keep the last direction - distance_to_next_wp = 0.0f; - - } - - - //_thrust = _thrust + 0.f; - - // yaw rate control - //yaw_setpoint += _manual_control_setpoint.roll*0.1f; + float speed = vel(0); + float speed_sp = distance_to_next_wp*_param_usv_pos_p.get() ; // Adjust the setpoint to take the shortest path float heading_error = desired_heading - yaw; if (abs(heading_error)>M_PI_F){ float new_heading_error = 2*M_PI_F-abs(heading_error); desired_heading = -sign(heading_error)*new_heading_error+yaw; + heading_error = new_heading_error; } + if (_current_waypoint != current_waypoint) { + _currentState = GuidanceState::TURNING; + } - dbg.value = distance_to_next_wp; - orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + //once position reached + if (distance_to_next_wp<_param_usv_dist_epsi.get()) { + _currentState = GuidanceState::GOAL_REACHED; + } - // Set the thrust to 0 when the heading error is too high - /*if (fabsf(heading_error)>=0.5f){ - distance_to_next_wp = 0; - }*/ + // adjust setpoints according to the step ongoing + switch (_currentState) { + case GuidanceState::TURNING: + speed_sp = 0.0f; + if (fabsf(heading_error) < _param_usv_yaw_epsi.get()) { + _currentState = GuidanceState::DRIVING; + } + break; - // Speed control - float _thrust = pid_calculate(&_velocity_pid, distance_to_next_wp, vel(0), 0, dt); - _thrust = _thrust * (1-(heading_error/M_PI_F)); - _thrust = math::constrain(_thrust, -1.0f, 0.9f); + case GuidanceState::DRIVING: { + break; + } + + case GuidanceState::GOAL_REACHED: + desired_heading = yaw; // keep the last direction + speed_sp = 0.0f; + break; + } + dbg.value = heading_error; + orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + // Speed control + speed_sp = math::constrain(speed_sp, 0.0f, _param_usv_speed_max.get()); + float _thrust = _param_usv_speed_ffg.get()*speed + pid_calculate(&_velocity_pid, speed_sp, speed, 0, dt); + _thrust = math::constrain(_thrust, -1.0f, 0.9f); + // direction control float _torque_sp = pid_calculate(&_yaw_rate_pid, desired_heading, yaw, 0, dt); _torque_sp = math::constrain(_torque_sp, -1.0f, 1.0f); + // publish torque and thurst commands v_thrust_sp.xyz[0] = _thrust; v_torque_sp.xyz[2] = -_torque_sp; - _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); _vehicle_torque_setpoint_pub.publish(v_torque_sp); + //_differential_drive_guidance.computeGuidance(yaw,vel(0),dt); + _current_waypoint = current_waypoint; } } else if (_armed && vehicle_control_mode.flag_control_manual_enabled) { diff --git a/src/modules/boat_pos_control/boat_pos_control.hpp b/src/modules/boat_pos_control/boat_pos_control.hpp index 198b47506ee4..7bee55c09845 100644 --- a/src/modules/boat_pos_control/boat_pos_control.hpp +++ b/src/modules/boat_pos_control/boat_pos_control.hpp @@ -67,6 +67,8 @@ #include #include +#include "differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp" + using matrix::Dcmf; using namespace matrix; @@ -124,14 +126,24 @@ class BoatPosControl : public ModuleBase, public ModuleParams, p manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ vehicle_attitude_s _vehicle_att{}; float yaw_setpoint; + GuidanceState _currentState; ///< The current state of guidance. + matrix::Vector2d _current_waypoint; ///< The current waypoint. + bool _armed = false; bool _position_ctrl_ena = false; vehicle_control_mode_s vehicle_control_mode; + DifferentialDriveGuidance _differential_drive_guidance{this}; DEFINE_PARAMETERS((ParamFloat) _param_usv_speed_p, (ParamFloat) _param_usv_yaw_rate_p, - (ParamFloat) _param_usv_dist_epsi + (ParamFloat) _param_usv_yaw_i, + (ParamFloat) _param_usv_yaw_epsi, + (ParamFloat) _param_usv_dist_epsi, + (ParamFloat) _param_usv_pos_p, + (ParamFloat) _param_usv_speed_max, + (ParamFloat) _param_usv_speed_ffg + ) diff --git a/src/modules/boat_pos_control/module.yaml b/src/modules/boat_pos_control/module.yaml index 3f391f326b7f..65fe3d78ed32 100644 --- a/src/modules/boat_pos_control/module.yaml +++ b/src/modules/boat_pos_control/module.yaml @@ -3,6 +3,24 @@ module_name: Boat Position Control parameters: - group: Boat Position Control definitions: + USV_POS_P: + description: + short: Position proportional gain + type: float + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 1 + USV_SPEED_MAX: + description: + short: Maximum linear speed + type: float + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 4 USV_SPEED_P: description: short: Speed proportional gain @@ -12,6 +30,15 @@ parameters: increment: 0.001 decimal: 3 default: 1 + USV_SPEED_FFG: + description: + short: Speed controller feedforward gain + type: float + min: 0.001 + max: 10 + increment: 0.001 + decimal: 3 + default: 0.14 USV_YAW_RATE_P: description: short: Yaw rate proportional gain @@ -21,6 +48,24 @@ parameters: increment: 0.001 decimal: 3 default: 0.1 + USV_YAW_RATE_I: + description: + short: Yaw rate integral gain + type: float + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.5 + USV_YAW_EPSI: + description: + short: Heading accuracy + type: float + min: 0.001 + max: 10 + increment: 0.001 + decimal: 3 + default: 0.1 USV_DIST_EPSI: description: short: Accuracy of distance to target @@ -29,5 +74,5 @@ parameters: max: 10 increment: 0.001 decimal: 3 - default: 0.01 + default: 1 From 196af8e11830b5b056d2d684bed5f23c58ced6b5 Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Sun, 7 Apr 2024 18:15:04 +0200 Subject: [PATCH 09/12] adjusting manual setpoint until the range is properly defined in the Mavlink interface --- src/modules/boat_pos_control/boat_pos_control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/boat_pos_control/boat_pos_control.cpp b/src/modules/boat_pos_control/boat_pos_control.cpp index 96178cbb022e..e33b17f0fe7f 100644 --- a/src/modules/boat_pos_control/boat_pos_control.cpp +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -224,7 +224,7 @@ void BoatPosControl::Run() } else if (_armed && vehicle_control_mode.flag_control_manual_enabled) { _manual_control_setpoint_sub.copy(&_manual_control_setpoint); - v_thrust_sp.xyz[0] = _manual_control_setpoint.throttle; + v_thrust_sp.xyz[0] = (_manual_control_setpoint.throttle + 1)/2; // adjusting manual setpoint until the range is properly defined in the Mavlink interface _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); v_torque_sp.xyz[2] = -_manual_control_setpoint.roll*0.1f; From 3b09c1fce7f166a2cb72c4450ef4c7d498eddab8 Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Sun, 7 Apr 2024 23:07:33 +0200 Subject: [PATCH 10/12] fixing startup script --- ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt | 2 +- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 4f8c3ae8a147..21eed06ff700 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -83,7 +83,7 @@ px4_add_romfs_files( 4009_gz_r1_rover 4010_gz_x500_mono_cam 4011_gz_lawnmower - 4009_gz_r1_rover + 4012_gz_boat 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 34b255e2555f..de9939e835fe 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -74,6 +74,9 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL) rc.rover_defaults rc.boat_defaults # hack + + rc.boat_differential_apps + rc.boat_differential_defaults ) endif() From da5d35422a6dc24c492c7647344ce6a022f4faff Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Sun, 21 Jul 2024 22:31:22 +0200 Subject: [PATCH 11/12] update the config to build the module for fmu v6c --- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 4 ++++ .../px4fmu_common/init.d/airframes/CMakeLists.txt | 6 ++++++ boards/px4/fmu-v6c/default.px4board | 1 + src/modules/boat_pos_control/boat_pos_control.cpp | 2 +- src/modules/boat_pos_control/boat_pos_control.hpp | 14 ++++++++++++-- 5 files changed, 24 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index de9939e835fe..6580ab0fcfed 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -74,7 +74,11 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL) rc.rover_defaults rc.boat_defaults # hack + ) +endif() +if(CONFIG_MODULES_BOAT_POS_CONTROL) + px4_add_romfs_files( rc.boat_differential_apps rc.boat_differential_defaults ) diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 2249a14cd3d9..6b268d10c88a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -149,6 +149,12 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) ) endif() +if(CONFIG_MODULES_BOAT_POS_CONTROL) + px4_add_romfs_files( + 70001_boat_mot_catamaran + ) +endif() + if(CONFIG_MODULES_UUV_ATT_CONTROL) px4_add_romfs_files( # [60000, 61000] (Unmanned) Underwater Robots diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index 148e6326fb26..4c81d0f008a5 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -63,6 +63,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_BOAT_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y diff --git a/src/modules/boat_pos_control/boat_pos_control.cpp b/src/modules/boat_pos_control/boat_pos_control.cpp index e33b17f0fe7f..9d4dca1d043b 100644 --- a/src/modules/boat_pos_control/boat_pos_control.cpp +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -224,7 +224,7 @@ void BoatPosControl::Run() } else if (_armed && vehicle_control_mode.flag_control_manual_enabled) { _manual_control_setpoint_sub.copy(&_manual_control_setpoint); - v_thrust_sp.xyz[0] = (_manual_control_setpoint.throttle + 1)/2; // adjusting manual setpoint until the range is properly defined in the Mavlink interface + v_thrust_sp.xyz[0] = _manual_control_setpoint.throttle ; // adjusting manual setpoint until the range is properly defined in the Mavlink interface _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); v_torque_sp.xyz[2] = -_manual_control_setpoint.roll*0.1f; diff --git a/src/modules/boat_pos_control/boat_pos_control.hpp b/src/modules/boat_pos_control/boat_pos_control.hpp index 7bee55c09845..36a32bb27b42 100644 --- a/src/modules/boat_pos_control/boat_pos_control.hpp +++ b/src/modules/boat_pos_control/boat_pos_control.hpp @@ -67,7 +67,7 @@ #include #include -#include "differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp" +//#include "differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp" using matrix::Dcmf; @@ -75,6 +75,16 @@ using namespace matrix; using namespace time_literals; +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + TURNING, ///< The vehicle is currently turning. + DRIVING, ///< The vehicle is currently driving straight. + GOAL_REACHED ///< The vehicle has reached its goal. +}; + + class BoatPosControl : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -133,7 +143,7 @@ class BoatPosControl : public ModuleBase, public ModuleParams, p bool _armed = false; bool _position_ctrl_ena = false; vehicle_control_mode_s vehicle_control_mode; - DifferentialDriveGuidance _differential_drive_guidance{this}; + //DifferentialDriveGuidance _differential_drive_guidance{this}; DEFINE_PARAMETERS((ParamFloat) _param_usv_speed_p, (ParamFloat) _param_usv_yaw_rate_p, From 537791f0403e10765decd57dd38446718caba48c Mon Sep 17 00:00:00 2001 From: Ronan Blanchard Date: Sun, 21 Jul 2024 23:39:32 +0200 Subject: [PATCH 12/12] adding Thrust and torque limit parameters --- .../boat_pos_control/boat_pos_control.cpp | 4 ++-- .../boat_pos_control/boat_pos_control.hpp | 5 ++++- src/modules/boat_pos_control/module.yaml | 18 ++++++++++++++++++ 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/src/modules/boat_pos_control/boat_pos_control.cpp b/src/modules/boat_pos_control/boat_pos_control.cpp index 9d4dca1d043b..3405a261b818 100644 --- a/src/modules/boat_pos_control/boat_pos_control.cpp +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -224,10 +224,10 @@ void BoatPosControl::Run() } else if (_armed && vehicle_control_mode.flag_control_manual_enabled) { _manual_control_setpoint_sub.copy(&_manual_control_setpoint); - v_thrust_sp.xyz[0] = _manual_control_setpoint.throttle ; // adjusting manual setpoint until the range is properly defined in the Mavlink interface + v_thrust_sp.xyz[0] = math::constrain(_manual_control_setpoint.throttle, -_param_usv_thr_max.get(), _param_usv_thr_max.get()) ; // adjusting manual setpoint until the range is properly defined in the Mavlink interface _vehicle_thrust_setpoint_pub.publish(v_thrust_sp); - v_torque_sp.xyz[2] = -_manual_control_setpoint.roll*0.1f; + v_torque_sp.xyz[2] = math::constrain(-_manual_control_setpoint.roll,-_param_usv_trq_max.get(),_param_usv_trq_max.get()); _vehicle_torque_setpoint_pub.publish(v_torque_sp); diff --git a/src/modules/boat_pos_control/boat_pos_control.hpp b/src/modules/boat_pos_control/boat_pos_control.hpp index 36a32bb27b42..9845e0be4a55 100644 --- a/src/modules/boat_pos_control/boat_pos_control.hpp +++ b/src/modules/boat_pos_control/boat_pos_control.hpp @@ -152,7 +152,10 @@ class BoatPosControl : public ModuleBase, public ModuleParams, p (ParamFloat) _param_usv_dist_epsi, (ParamFloat) _param_usv_pos_p, (ParamFloat) _param_usv_speed_max, - (ParamFloat) _param_usv_speed_ffg + (ParamFloat) _param_usv_speed_ffg, + (ParamFloat) _param_usv_thr_max, + (ParamFloat) _param_usv_trq_max + ) diff --git a/src/modules/boat_pos_control/module.yaml b/src/modules/boat_pos_control/module.yaml index 65fe3d78ed32..3fd4815f3c6e 100644 --- a/src/modules/boat_pos_control/module.yaml +++ b/src/modules/boat_pos_control/module.yaml @@ -75,4 +75,22 @@ parameters: increment: 0.001 decimal: 3 default: 1 + USV_THR_MAX: + description: + short: Thrust max + type: float + min: 0 + max: 1 + increment: 0.01 + decimal: 2 + default: 1 + USV_TRQ_MAX: + description: + short: Torque max + type: float + min: 0 + max: 1 + increment: 0.01 + decimal: 2 + default: 1