diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_boat b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_boat new file mode 100644 index 000000000000..525ca7b57ed3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_boat @@ -0,0 +1,31 @@ +#!/bin/sh +# @name Aion Robotics R1 Rover +# @type Rover +# @class Rover + +. ${R}etc/init.d/rc.boat_differential_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} + +param set-default SIM_GZ_EN 1 # Gazebo bridge + +# Simulated sensors +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 1 + +# Actuator mapping +param set-default SIM_GZ_WH_FUNC1 101 # right wheel +param set-default SIM_GZ_WH_MIN1 0 +param set-default SIM_GZ_WH_MAX1 200 +param set-default SIM_GZ_WH_DIS1 100 + +param set-default SIM_GZ_WH_FUNC2 102 # left wheel +param set-default SIM_GZ_WH_MIN2 0 +param set-default SIM_GZ_WH_MAX2 200 +param set-default SIM_GZ_WH_DIS2 100 + +param set-default SIM_GZ_WH_REV 1 # reverse right wheel diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 9277ad303947..21eed06ff700 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -83,6 +83,7 @@ px4_add_romfs_files( 4009_gz_r1_rover 4010_gz_x500_mono_cam 4011_gz_lawnmower + 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..6580ab0fcfed 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -77,6 +77,13 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL) ) endif() +if(CONFIG_MODULES_BOAT_POS_CONTROL) + px4_add_romfs_files( + rc.boat_differential_apps + rc.boat_differential_defaults + ) +endif() + if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) px4_add_romfs_files( rc.rover_differential_apps diff --git a/ROMFS/px4fmu_common/init.d/airframes/70001_boat_mot_catamaran b/ROMFS/px4fmu_common/init.d/airframes/70001_boat_mot_catamaran new file mode 100644 index 000000000000..7ac4d036c388 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/70001_boat_mot_catamaran @@ -0,0 +1,22 @@ +#!/bin/sh +# +# @name Motorized catamaran +# +# +# @type Boat +# @class Catamaran +# +# + +. ${R}etc/init.d/rc.boat_differential_defaults + +param set-default BAT1_N_CELLS 4 + +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 + +# Set geometry & output configration +param set-default RBCLW_ADDRESS 128 +param set-default RBCLW_FUNC1 101 +param set-default RBCLW_FUNC2 102 +param set-default RBCLW_REV 1 # reverse right wheels diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 66e29e32b4dd..6b268d10c88a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -145,6 +145,13 @@ endif() if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) px4_add_romfs_files( 50003_aion_robotics_r1_rover + 70001_boat_mot_catamaran + ) +endif() + +if(CONFIG_MODULES_BOAT_POS_CONTROL) + px4_add_romfs_files( + 70001_boat_mot_catamaran ) endif() diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_differential_apps b/ROMFS/px4fmu_common/init.d/rc.boat_differential_apps new file mode 100644 index 000000000000..acaba4f5e7a5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.boat_differential_apps @@ -0,0 +1,15 @@ +#!/bin/sh +# Standard apps for a differential drive rover. + +# Start the attitude and position estimator. +ekf2 start & +# +# Start Control Allocator +# +control_allocator start + +# Start boat position controller. +boat_pos_control start + +# Start Land Detector. +land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults b/ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults new file mode 100644 index 000000000000..2674f2719443 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.boat_differential_defaults @@ -0,0 +1,13 @@ +#!/bin/sh +# Boat parameters. + +set VEHICLE_TYPE boat + +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/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 1b7ddee5a569..5441203c9224 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_differential_apps +fi + # # Differential Rover setup. # 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/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 7dd7ce481a3f..4e1d4512c9a8 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..3405a261b818 --- /dev/null +++ b/src/modules/boat_pos_control/boat_pos_control.cpp @@ -0,0 +1,304 @@ +/**************************************************************************** + * + * 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(); + } + // 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 + 0, // Integral gain + 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 + _param_usv_yaw_i.get(), // Integral gain + 0, // Derivative gain + 1, // 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 + + /* advertise debug value */ + struct debug_key_value_s dbg; + 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); + + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + parameters_update(); + vehicle_attitude_poll(); + + 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; + + const Quatf q{_vehicle_att.q}; + float yaw = Eulerf(q).psi(); + + + + 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 (_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); + + 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)); + + + + 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); + 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; + } + + //once position reached + if (distance_to_next_wp<_param_usv_dist_epsi.get()) { + _currentState = GuidanceState::GOAL_REACHED; + } + + // 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; + + 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) { + _manual_control_setpoint_sub.copy(&_manual_control_setpoint); + 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] = math::constrain(-_manual_control_setpoint.roll,-_param_usv_trq_max.get(),_param_usv_trq_max.get()); + _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..9845e0be4a55 --- /dev/null +++ b/src/modules/boat_pos_control/boat_pos_control.hpp @@ -0,0 +1,165 @@ +/**************************************************************************** + * + * 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 +#include +#include + +#include +#include +#include + +//#include "differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp" + +using matrix::Dcmf; + +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: + 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)}; + 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)}; + + // 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. + 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; + 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_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, + (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 new file mode 100644 index 000000000000..3fd4815f3c6e --- /dev/null +++ b/src/modules/boat_pos_control/module.yaml @@ -0,0 +1,96 @@ +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 + type: float + min: 0.001 + max: 100 + 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 + type: float + min: 0.001 + max: 100 + 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 + type: float + min: 0.001 + max: 10 + 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 + diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessBoatDifferential.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessBoatDifferential.cpp new file mode 100644 index 000000000000..eac687021305 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessBoatDifferential.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 "ActuatorEffectivenessBoatDifferential.hpp" +#include + +using namespace matrix; + +bool +ActuatorEffectivenessBoatDifferential::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 ActuatorEffectivenessBoatDifferential::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/ActuatorEffectivenessBoatDifferential.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessBoatDifferential.hpp new file mode 100644 index 000000000000..ede11f07994f --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessBoatDifferential.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 ActuatorEffectivenessBoatDifferential: public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessBoatDifferential() = default; + virtual ~ActuatorEffectivenessBoatDifferential() = 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 "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 2406b81bf80b..e6c7d4569b9f 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 + 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 2ead22c57079..79bebb29e704 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -270,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 303316f1ef52..436475fc83a3 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 @@ -158,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 ]