diff --git a/ROMFS/px4fmu_common/init.d-posix/1020_hippocampus b/ROMFS/px4fmu_common/init.d-posix/1020_hippocampus deleted file mode 100644 index b7c0dacf4c..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/1020_hippocampus +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/sh -# -# @name Hippocampus UUV -# - -sh /etc/init.d/rc.mc_defaults - -set MIXER uuv_quad_x diff --git a/ROMFS/px4fmu_common/init.d-posix/1020_uuv_generic b/ROMFS/px4fmu_common/init.d-posix/1020_uuv_generic new file mode 100644 index 0000000000..baa5150d28 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/1020_uuv_generic @@ -0,0 +1,23 @@ +#!/bin/sh +# +# @name UUV +# + +sh /etc/init.d/rc.uuv_defaults + +if [ $AUTOCNF = yes ] +then + #Set data link loss failsafe mode (0: disabled, 4: Data Link Auto Recovery (CASA Outback Challenge rules)) + param set NAV_DLL_ACT 0 + + # disable circuit breaker for airspeed sensor + param set CBRK_AIRSPD_CHK 162128 + + #param set CBRK_GPSFAIL 240024 +fi + +set MAV_TYPE 12 +param set MAV_TYPE ${MAV_TYPE} + +set MIXER_FILE etc/mixers-sitl/uuv_x_sitl.main.mix +set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/1021_uuv_hippocampus b/ROMFS/px4fmu_common/init.d-posix/1021_uuv_hippocampus new file mode 100644 index 0000000000..b604075ce4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/1021_uuv_hippocampus @@ -0,0 +1,23 @@ +#!/bin/sh +# +# @name Hippocampus UUV +# + +sh /etc/init.d/rc.uuv_defaults + +if [ $AUTOCNF = yes ] +then + #Set data link loss failsafe mode (0: disabled, 4: Data Link Auto Recovery (CASA Outback Challenge rules)) + param set NAV_DLL_ACT 0 + + # disable circuit breaker for airspeed sensor + param set CBRK_AIRSPD_CHK 162128 + + #param set CBRK_GPSFAIL 240024 +fi + +set MAV_TYPE 12 +param set MAV_TYPE ${MAV_TYPE} + +set MIXER_FILE etc/mixers-sitl/uuv_x_sitl.main.mix +set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index f769ef4e4f..4446c6a658 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -46,6 +46,8 @@ px4_add_romfs_files( rc.thermal_cal rc.rover_apps rc.rover_defaults + rc.uuv_apps + rc.uuv_defaults rc.vehicle_setup rc.vtol_apps rc.vtol_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic b/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic new file mode 100644 index 0000000000..b31b0f5a83 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic @@ -0,0 +1,29 @@ +#!/bin/sh +# +# @name Generic Underwater Robot +# +# @type Underwater Robot +# @class Underwater Robot +# +# @board px4_fmu-v2 exclude +# +# @maintainer +# + +sh /etc/init.d/rc.uuv_defaults + +if [ $AUTOCNF = yes ] +then + #Set data link loss failsafe mode (0: disabled, 4: Data Link Auto Recovery (CASA Outback Challenge rules)) + param set NAV_DLL_ACT 0 + + # disable circuit breaker for airspeed sensor + param set CBRK_AIRSPD_CHK 162128 + + #param set CBRK_GPSFAIL 240024 +fi + +set MAV_TYPE 12 +param set MAV_TYPE ${MAV_TYPE} + +set MIXER uuv_x diff --git a/ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus b/ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus new file mode 100644 index 0000000000..30019aa2b8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus @@ -0,0 +1,29 @@ +#!/bin/sh +# +# @name HippoCampus UUV (Unmanned Underwater Vehicle) +# +# @type Underwater Robot +# @class Underwater Robot +# +# @board px4_fmu-v2 exclude +# +# @maintainer Daniel Duecker +# + +sh /etc/init.d/rc.uuv_defaults + +if [ $AUTOCNF = yes ] +then + #Set data link loss failsafe mode (0: disabled, 4: Data Link Auto Recovery (CASA Outback Challenge rules)) + param set NAV_DLL_ACT 0 + + # disable circuit breaker for airspeed sensor + param set CBRK_AIRSPD_CHK 162128 + + #param set CBRK_GPSFAIL 240024 +fi + +set MAV_TYPE 12 +param set MAV_TYPE ${MAV_TYPE} + +set MIXER uuv_x diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index f21c1c63bc..80d7c9f4a0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -146,4 +146,8 @@ px4_add_romfs_files( 50001_axialracing_ax10 50002_traxxas_stampede_2wd 50003_aion_robotics_r1_rover + + # [60000, 61000] (Unmanned) Underwater Robots + 60000_uuv_generic + 60001_uuv_hippocampus ) diff --git a/ROMFS/px4fmu_common/init.d/rc.uuv_apps b/ROMFS/px4fmu_common/init.d/rc.uuv_apps new file mode 100644 index 0000000000..5219f11986 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.uuv_apps @@ -0,0 +1,26 @@ +#!/bin/sh +# +# Standard apps for uuvs. Attitude/Position estimator, Attitude/Position control. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +############################################################################### +# Begin Estimator Group Selection # +############################################################################### + +ekf2 start + +############################################################################### +# End Estimator Group Selection # +############################################################################### + +# +# Start UUV Attitude Controller. +# +uuv_att_control start + +# +# Start UUV Land Detector. +# +land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.uuv_defaults b/ROMFS/px4fmu_common/init.d/rc.uuv_defaults new file mode 100644 index 0000000000..804e41e3f1 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.uuv_defaults @@ -0,0 +1,35 @@ +#!/bin/sh +# +# UUV default parameters. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +set VEHICLE_TYPE uuv + +if [ $AUTOCNF = yes ] +then + param set PWM_MAX 1950 + param set PWM_MIN 1050 + param set PWM_DISARMED 1500 + +fi + +# +# PWM Hz - 50 Hz is the normal rate in RC cars, boats etc, +# higher rates may damage analog servos. +# +set PWM_RATE 50 + +# +# Enable servo output on pins 1-4 +set PWM_OUT 1234 +set PWM_DISARMED 1500 + + +# +# This is the gimbal pass mixer. +# +set MIXER_AUX pass +set PWM_AUX_OUT 1234 +set PWM_AUX_RATE 50 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 20a60edf1b..b6002edc87 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -147,6 +147,26 @@ then sh /etc/init.d/rc.vtol_apps fi +# +# UUV setup +# +if [ $VEHICLE_TYPE = uuv ] +then + if [ $MIXER = none ] + then + echo "UUV mixer undefined" + fi + + # Load mixer and configure outputs. + sh /etc/init.d/rc.interface + + # Start standard vtol apps. + sh /etc/init.d/rc.uuv_apps + +fi + + + # # Generic setup (autostart ID not found). # diff --git a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt index 312dd63b97..85f1e6613a 100644 --- a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt @@ -37,4 +37,5 @@ px4_add_romfs_files( rover_sitl.main.mix standard_vtol_sitl.main.mix tiltrotor_sitl.main.mix + uuv_x_sitl.main.mix ) diff --git a/ROMFS/px4fmu_common/mixers-sitl/uuv_x_sitl.main.mix b/ROMFS/px4fmu_common/mixers-sitl/uuv_x_sitl.main.mix new file mode 100644 index 0000000000..ab27c467e0 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers-sitl/uuv_x_sitl.main.mix @@ -0,0 +1,25 @@ +Mixer for Unmanned Underwater Vehicles (UUV) with X-Motor configuration +========================================================================== + +# @board px4_fmu-v2 exclude + +M: 4 +S: 0 0 -4000 -4000 0 -4000 4000 +S: 0 1 -4000 -4000 0 -4000 4000 +S: 0 2 +4000 +4000 0 -4000 4000 +S: 0 3 +4000 +4000 0 -4000 4000 +M: 4 +S: 0 0 +4000 +4000 0 -4000 4000 +S: 0 1 -4000 -4000 0 -4000 4000 +S: 0 2 -4000 -4000 0 -4000 4000 +S: 0 3 +4000 +4000 0 -4000 4000 +M: 4 +S: 0 0 -4000 -4000 0 -4000 4000 +S: 0 1 +4000 +4000 0 -4000 4000 +S: 0 2 -4000 -4000 0 -4000 4000 +S: 0 3 +4000 +4000 0 -4000 4000 +M: 4 +S: 0 0 +4000 +4000 0 -4000 4000 +S: 0 1 +4000 +4000 0 -4000 4000 +S: 0 2 +4000 +4000 0 -4000 4000 +S: 0 3 +4000 +4000 0 -4000 4000 diff --git a/ROMFS/px4fmu_common/mixers/CMakeLists.txt b/ROMFS/px4fmu_common/mixers/CMakeLists.txt index f3a5022c51..e1a8738817 100644 --- a/ROMFS/px4fmu_common/mixers/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers/CMakeLists.txt @@ -84,4 +84,5 @@ px4_add_romfs_files( wingwing.main.mix zmr250.main.mix TF-AutoG2.main.mix + uuv_x.main.mix ) diff --git a/ROMFS/px4fmu_common/mixers/uuv_quad_x.mix b/ROMFS/px4fmu_common/mixers/uuv_quad_x.mix deleted file mode 100644 index eda24a96d9..0000000000 --- a/ROMFS/px4fmu_common/mixers/uuv_quad_x.mix +++ /dev/null @@ -1,22 +0,0 @@ -# @board px4_fmu-v2 exclude - -M: 4 -S: 0 0 -10000 -10000 0 -10000 10000 -S: 0 1 -10000 -10000 0 -10000 10000 -S: 0 2 +10000 +10000 0 -10000 10000 -S: 0 3 +10000 +10000 0 -10000 10000 -M: 4 -S: 0 0 +10000 +10000 0 -10000 10000 -S: 0 1 -10000 -10000 0 -10000 10000 -S: 0 2 -10000 -10000 0 -10000 10000 -S: 0 3 +10000 +10000 0 -10000 10000 -M: 4 -S: 0 0 -10000 -10000 0 -10000 10000 -S: 0 1 +10000 +10000 0 -10000 10000 -S: 0 2 -10000 -10000 0 -10000 10000 -S: 0 3 +10000 +10000 0 -10000 10000 -M: 4 -S: 0 0 +10000 +10000 0 -10000 10000 -S: 0 1 +10000 +10000 0 -10000 10000 -S: 0 2 +10000 +10000 0 -10000 10000 -S: 0 3 +10000 +10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/uuv_x.main.mix b/ROMFS/px4fmu_common/mixers/uuv_x.main.mix new file mode 100644 index 0000000000..bebc454e5d --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/uuv_x.main.mix @@ -0,0 +1,25 @@ +Mixer for Unmanned Underwater Vehicles (UUV) with X-Motor configuration +========================================================================== + +# @board px4_fmu-v2 exclude + +M: 4 +S: 0 0 -4000 -4000 0 -10000 10000 +S: 0 1 -4000 -4000 0 -10000 10000 +S: 0 2 +4000 +4000 0 -10000 10000 +S: 0 3 +4000 +4000 0 -10000 10000 +M: 4 +S: 0 0 -4000 -4000 0 -10000 10000 +S: 0 1 +4000 +4000 0 -10000 10000 +S: 0 2 +4000 +4000 0 -10000 10000 +S: 0 3 -4000 -4000 0 -10000 10000 +M: 4 +S: 0 0 -4000 -4000 0 -10000 10000 +S: 0 1 +4000 +4000 0 -10000 10000 +S: 0 2 -4000 -4000 0 -10000 10000 +S: 0 3 +4000 +4000 0 -10000 10000 +M: 4 +S: 0 0 -4000 -4000 0 -10000 10000 +S: 0 1 -4000 -4000 0 -10000 10000 +S: 0 2 -4000 -4000 0 -10000 10000 +S: 0 3 -4000 -4000 0 -10000 10000 diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index c84e429e9f..5d5eb340b0 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -82,6 +82,7 @@ px4_add_board( temperature_compensation vmount vtol_att_control + uuv_att_control SYSTEMCMDS bl_update config diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 396492ca93..5b2b9e8033 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -48,6 +48,8 @@ px4_add_board( temperature_compensation vmount vtol_att_control + uuv_att_control + SYSTEMCMDS #config #dumpfile diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index dd2adc7196..ffeb50b58e 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -64,7 +64,8 @@ set(models none shell if750a iris iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps solo typhoon_h480 plane plane_catapult standard_vtol tailsitter tiltrotor - hippocampus rover) + rover + uuv_hippocampus) set(all_posix_vmd_make_targets) foreach(viewer ${viewers}) foreach(debugger ${debuggers}) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2f10a84751..71c8849cd5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1370,6 +1370,10 @@ void MavlinkReceiver::fill_thrust(float *thrust_body_array, uint8_t vehicle_type thrust_body_array[2] = -thrust; break; + case MAV_TYPE_SUBMARINE: + thrust_body_array[0] = thrust; + break; + case MAV_TYPE_VTOL_DUOROTOR: case MAV_TYPE_VTOL_QUADROTOR: case MAV_TYPE_VTOL_TILTROTOR: diff --git a/src/modules/uuv_att_control/CMakeLists.txt b/src/modules/uuv_att_control/CMakeLists.txt new file mode 100644 index 0000000000..37dc0d728c --- /dev/null +++ b/src/modules/uuv_att_control/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2020 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__uuv_att_control + MAIN uuv_att_control + STACK_MAIN 1200 + COMPILE_FLAGS + SRCS + uuv_att_control.cpp + ) diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp new file mode 100644 index 0000000000..11de7b372f --- /dev/null +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -0,0 +1,452 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +/** + * + * This module is a modification of the fixed wing / rover module and it is designed for unmanned underwater vehicles (UUV). + * It has been developed starting from the fw module, simplified and improved with dedicated items. + * + * All the acknowledgments and credits for the fw wing/rover app are reported in those files. + * + * @author Daniel Duecker + * @author Philipp Hastedt + * @author Tim Hansen + */ + +#include "uuv_att_control.hpp" + + +#define ACTUATOR_PUBLISH_PERIOD_MS 4 + + +/** + * UUV attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int uuv_att_control_main(int argc, char *argv[]); + + +UUVAttitudeControl::UUVAttitudeControl(): + ModuleParams(nullptr), + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "gnda_dt")) +{ +} + +UUVAttitudeControl::~UUVAttitudeControl() +{ + perf_free(_loop_perf); +} + +void UUVAttitudeControl::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } +} + +void UUVAttitudeControl::vehicle_control_mode_poll() +{ + bool updated = false; + orb_check(_vcontrol_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); + } +} + +void UUVAttitudeControl::manual_control_setpoint_poll() +{ + bool updated = false; + orb_check(_manual_control_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual); + } +} + +void UUVAttitudeControl::vehicle_attitude_setpoint_poll() +{ + bool updated = false; + orb_check(_vehicle_attitude_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _vehicle_attitude_sp_sub, &_vehicle_attitude_sp); + } +} + + +void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, float thrust_u) +{ + if (PX4_ISFINITE(roll_u)) { + roll_u = math::constrain(roll_u, -1.0f, 1.0f); + _actuators.control[actuator_controls_s::INDEX_ROLL] = roll_u; + + } else { + _actuators.control[actuator_controls_s::INDEX_ROLL] = 0.0f; + + if (loop_counter % 10 == 0) { + PX4_INFO("roll_u %.4f", (double)roll_u); + } + } + + if (PX4_ISFINITE(pitch_u)) { + pitch_u = math::constrain(pitch_u, -1.0f, 1.0f); + _actuators.control[actuator_controls_s::INDEX_PITCH] = pitch_u; + + } else { + _actuators.control[actuator_controls_s::INDEX_PITCH] = 0.0f; + + if (loop_counter % 10 == 0) { + PX4_INFO("pitch_u %.4f", (double)pitch_u); + } + } + + if (PX4_ISFINITE(yaw_u)) { + yaw_u = math::constrain(yaw_u, -1.0f, 1.0f); + _actuators.control[actuator_controls_s::INDEX_YAW] = yaw_u; + + } else { + _actuators.control[actuator_controls_s::INDEX_YAW] = 0.0f; + + if (loop_counter % 10 == 0) { + PX4_INFO("yaw_u %.4f", (double)yaw_u); + } + } + + if (PX4_ISFINITE(thrust_u)) { + thrust_u = math::constrain(thrust_u, -1.0f, 1.0f); + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = thrust_u; + + } else { + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f; + + if (loop_counter % 10 == 0) { + PX4_INFO("thrust_u %.4f", (double)thrust_u); + } + } +} + + +void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp) +{ + /** Geometric Controller + * + * based on + * D. Mellinger, V. Kumar, "Minimum Snap Trajectory Generation and Control for Quadrotors", IEEE ICRA 2011, pp. 2520-2525. + * D. A. Duecker, A. Hackbarth, T. Johannink, E. Kreuzer, and E. Solowjow, “Micro Underwater Vehicle Hydrobatics: A SubmergedFuruta Pendulum,” IEEE ICRA 2018, pp. 7498–7503. + */ + + + Eulerf euler_angles(matrix::Quatf(att.q)); + + float roll_u; + float pitch_u; + float yaw_u; + float thrust_u; + + float roll_body = _vehicle_attitude_sp.roll_body; + float pitch_body = _vehicle_attitude_sp.pitch_body; + float yaw_body = _vehicle_attitude_sp.yaw_body; + + /* get attitude setpoint rotational matrix */ + Dcmf _rot_des = Eulerf(roll_body, pitch_body, yaw_body); + + /* get current rotation matrix from control state quaternions */ + Quatf q_att(att.q[0], att.q[1], att.q[2], att.q[3]); + Matrix3f _rot_att = q_att.to_dcm(); + + Vector3f e_R_vec; + Vector3f torques; + Vector3f omega; + + /* Compute matrix: attitude error */ + Matrix3f e_R = (_rot_des.transpose() * _rot_att - _rot_att.transpose() * _rot_des) * 0.5; + + /* vee-map the error to get a vector instead of matrix e_R */ + e_R_vec(0) = e_R(2, 1); /**< Roll */ + e_R_vec(1) = e_R(0, 2); /**< Pitch */ + e_R_vec(2) = e_R(1, 0); /**< Yaw */ + + omega(0) = _angular_velocity.xyz[0] - 0.0f; + omega(1) = _angular_velocity.xyz[1] - 0.0f; + omega(2) = _angular_velocity.xyz[2] - 0.0f; + + /**< P-Control */ + torques(0) = - e_R_vec(0) * _param_roll_p.get(); /**< Roll */ + torques(1) = - e_R_vec(1) * _param_pitch_p.get(); /**< Pitch */ + torques(2) = - e_R_vec(2) * _param_yaw_p.get(); /**< Yaw */ + + /**< PD-Control */ + torques(0) = torques(0) - omega(0) * _param_roll_d.get(); /**< Roll */ + torques(1) = torques(1) - omega(1) * _param_pitch_d.get(); /**< Pitch */ + torques(2) = torques(2) - omega(2) * _param_yaw_d.get(); /**< Yaw */ + + roll_u = torques(0); + pitch_u = torques(1); + yaw_u = torques(2); + + //Quatf q_att(_att.q[0], _att.q[1], _att.q[2], _att.q[3]); + //Matrix3f _rot_att = q_att.to_dcm(); + /** Vector3f current_velocity_boat; + + current_velocity_boat(0) = _local_pos.vx; + current_velocity_boat(1) = _local_pos.vy; + current_velocity_boat(2) = _local_pos.vz; + + current_velocity_boat = q_att.to_dcm() * current_velocity_boat; + */ + + // take thrust as + thrust_u = _param_direct_thrust.get(); + + constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_u); + /* Geometric Controller END*/ +} + + +void UUVAttitudeControl::run() +{ + _vehicle_attitude_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _angular_velocity_sub = orb_subscribe(ORB_ID(vehicle_angular_velocity)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + + + /* rate limit control mode updates to 5Hz */ + orb_set_interval(_vcontrol_mode_sub, 200); + + parameters_update(true); + + /* wakeup source(s) */ + px4_pollfd_struct_t fds[3]; + + /* Setup of loop */ + fds[0].fd = _vehicle_attitude_sub; + fds[0].events = POLLIN; + fds[1].fd = _manual_control_sub; + fds[1].events = POLLIN; + fds[2].fd = _sensor_combined_sub; + fds[2].events = POLLIN; + + + while (!should_exit()) { + /* wait for up to 500ms for data */ + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + /* check vehicle control mode for changes to publication state */ + vehicle_control_mode_poll(); + vehicle_attitude_setpoint_poll(); + + /* update parameters from storage */ + parameters_update(); + + /* only run controller if attitude changed */ + if (fds[0].revents & POLLIN) { + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f || fabsf(deltaT) < 0.00001f || !PX4_ISFINITE(deltaT)) { + deltaT = 0.01f; + } + + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_attitude); + orb_copy(ORB_ID(vehicle_angular_velocity), _angular_velocity_sub, &_angular_velocity); + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + + vehicle_attitude_setpoint_poll(); + vehicle_control_mode_poll(); + manual_control_setpoint_poll(); + + + /* Run geometric attitude controllers if NOT manual mode*/ + if (!_vcontrol_mode.flag_control_manual_enabled + && _vcontrol_mode.flag_control_attitude_enabled + && _vcontrol_mode.flag_control_rates_enabled) { + + int input_mode = _param_input_mode.get(); + + // if (input_mode == 0) // process incoming vehicles setpoint data --> nothing to do + if (input_mode == 1) { // process manual data + _vehicle_attitude_sp.roll_body = _param_direct_roll.get(); + _vehicle_attitude_sp.pitch_body = _param_direct_pitch.get(); + _vehicle_attitude_sp.yaw_body = _param_direct_yaw.get(); + _vehicle_attitude_sp.thrust_body[0] = _param_direct_thrust.get(); + } + + /* Geometric Control*/ + control_attitude_geo(_vehicle_attitude, _vehicle_attitude_sp); + } + } + + loop_counter++; + perf_end(_loop_perf); + + /* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */ + if (fds[1].revents & POLLIN) { + // This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep + // returning immediately and this loop will eat up resources. + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual); + + if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) { + /* manual/direct control */ + constrain_actuator_commands(_manual.y, -_manual.x, _manual.r, _manual.z); + } + + } + + if (fds[2].revents & POLLIN) { + + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); + + _actuators.timestamp = hrt_absolute_time(); + + /* Only publish if any of the proper modes are enabled */ + if (_vcontrol_mode.flag_control_manual_enabled || + _vcontrol_mode.flag_control_attitude_enabled) { + /* publish the actuator controls */ + _actuator_controls_pub.publish(_actuators); + + } + } + } + + orb_unsubscribe(_vcontrol_mode_sub); + orb_unsubscribe(_manual_control_sub); + orb_unsubscribe(_vehicle_attitude_sub); + orb_unsubscribe(_local_pos_sub); + orb_unsubscribe(_sensor_combined_sub); + + warnx("exiting.\n"); +} + +int UUVAttitudeControl::task_spawn(int argc, char *argv[]) +{ + /* start the task */ + _task_id = px4_task_spawn_cmd("uuv_att_ctrl", + SCHED_DEFAULT, + SCHED_PRIORITY_ATTITUDE_CONTROL, + 1700, // maybe switch to 1500 + (px4_main_t)&UUVAttitudeControl::run_trampoline, + nullptr); + + if (_task_id < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +UUVAttitudeControl *UUVAttitudeControl::instantiate(int argc, char *argv[]) +{ + + if (argc > 0) { + PX4_WARN("Command 'start' takes no arguments."); + return nullptr; + } + + UUVAttitudeControl *instance = new UUVAttitudeControl(); + + if (instance == nullptr) { + PX4_ERR("Failed to instantiate UUVAttitudeControl object"); + } + + return instance; +} + +int UUVAttitudeControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + + +int UUVAttitudeControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Controls the attitude of an unmanned underwater vehicle (UUV). + +Publishes `actuator_controls_0` messages at a constant 250Hz. + +### Implementation +Currently, this implementation supports only a few modes: + + * Full manual: Roll, pitch, yaw, and throttle controls are passed directly through to the actuators + * Auto mission: The uuv runs missions + +### Examples +CLI usage example: +$ uuv_att_control start +$ uuv_att_control status +$ uuv_att_control stop + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("uuv_att_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start") + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int uuv_att_control_main(int argc, char *argv[]) +{ + return UUVAttitudeControl::main(argc, argv); +} diff --git a/src/modules/uuv_att_control/uuv_att_control.hpp b/src/modules/uuv_att_control/uuv_att_control.hpp new file mode 100644 index 0000000000..0df6c129b7 --- /dev/null +++ b/src/modules/uuv_att_control/uuv_att_control.hpp @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +/** + * + * This module is a modification of the rover attitide control module and is designed for the + * TUHH hippocampus. + * + * All the acknowledgments and credits for the fw wing app are reported in those files. + * + * @author Daniel Duecker + * @author Philipp Hastedt + * @author Tim Hansen + */ + + + +#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 + + + + +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Matrix3f; +using matrix::Vector3f; +using matrix::Dcmf; + +using uORB::SubscriptionData; + +class UUVAttitudeControl: public ModuleBase, public ModuleParams +{ +public: + UUVAttitudeControl(); + ~UUVAttitudeControl(); + + UUVAttitudeControl(const UUVAttitudeControl &) = delete; + UUVAttitudeControl operator=(const UUVAttitudeControl &other) = delete; + + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static UUVAttitudeControl *instantiate(int argc, char *argv[]); + + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::run() */ + void run() override; + + +// int start(); +// bool task_running() { return _task_running; } + +private: + uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; + uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_0)}; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + + int _vehicle_attitude_sp_sub{-1}; /**< vehicle attitude setpoint */ + int _battery_status_sub{-1}; /**< battery status subscription */ + int _vehicle_attitude_sub{-1}; /**< control state subscription */ + int _angular_velocity_sub{-1}; /**< vehicle angular velocity subscription */ + int _local_pos_sub{-1}; /**< local position subscription */ + int _manual_control_sub{-1}; /**< notification of manual control updates */ + int _vcontrol_mode_sub{-1}; /**< vehicle status subscription */ + int _sensor_combined_sub{-1}; /**< sensor combined subscription */ + + actuator_controls_s _actuators {}; /**< actuator control inputs */ + manual_control_setpoint_s _manual {}; /**< r/c channel data */ + vehicle_attitude_s _vehicle_attitude {}; /**< control state */ + vehicle_angular_velocity_s _angular_velocity{}; /**< angular velocity */ + vehicle_attitude_setpoint_s _vehicle_attitude_sp {};/**< vehicle attitude setpoint */ + vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */ + sensor_combined_s _sensor_combined{}; + vehicle_local_position_s _local_pos{}; /**< vehicle local position */ + + + SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + hrt_abstime _control_position_last_called{0}; /**) _param_roll_p, + (ParamFloat) _param_roll_d, + (ParamFloat) _param_pitch_p, + (ParamFloat) _param_pitch_d, + (ParamFloat) _param_yaw_p, + (ParamFloat) _param_yaw_d, + // control/input modes + (ParamInt) _param_input_mode, + // direct access to inputs + (ParamFloat) _param_direct_roll, + (ParamFloat) _param_direct_pitch, + (ParamFloat) _param_direct_yaw, + (ParamFloat) _param_direct_thrust + ) + + /** + * Update our local parameter cache. + */ + void parameters_update(bool force = false); + + void manual_control_setpoint_poll(); + void position_setpoint_triplet_poll(); + void vehicle_control_mode_poll(); + void vehicle_attitude_poll(); + void vehicle_attitude_setpoint_poll(); + void vehicle_local_position_poll(); + + /** + * Control Attitude + */ + void control_attitude_geo(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp); + + void control_attitude_pid(const vehicle_attitude_s &att, const vehicle_attitude_setpoint_s &att_sp, float deltaT); + void constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, float thrust_u); + + +}; diff --git a/src/modules/uuv_att_control/uuv_att_control_params.c b/src/modules/uuv_att_control/uuv_att_control_params.c new file mode 100644 index 0000000000..d25d8e9d43 --- /dev/null +++ b/src/modules/uuv_att_control/uuv_att_control_params.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file uuv_att_control_params.c + * + * Parameters defined by the attitude control task for unmanned underwater vehicles (UUVs) + * + * This is a modification of the fixed wing/ground rover params and it is designed for ground rovers. + * It has been developed starting from the fw module, simplified and improved with dedicated items. + * + * All the ackowledgments and credits for the fw wing/rover app are reported in those files. + * + * @author Daniel Duecker + */ + +/* + * Controller parameters, accessible via MAVLink + * + */ + +// Roll gains +/** + * Roll proportional gain + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_ROLL_P, 4.0f); + +/** + * Roll differential gain + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_ROLL_D, 1.5f); + + +// Pitch gains +/** + * Pitch proportional gain + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_PITCH_P, 4.0f); + +/** + * Pitch differential gain + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_PITCH_D, 2.0f); + + +// Yaw gains +/** + * Yawh proportional gain + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_YAW_P, 4.0f); + +/** + * Yaw differential gain + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_YAW_D, 2.0f); + + +// Input Modes +/** + * Select Input Mode + * + * @value 0 use Attitude Setpoints + * @value 1 Direct Feedthrough + * @group UUV Attitude Control + */ +PARAM_DEFINE_INT32(UUV_INPUT_MODE, 0); + +/** + * Direct roll input + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_DIRCT_ROLL, 0.0f); + +/** + * Direct pitch input + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_DIRCT_PITCH, 0.0f); + +/** + * Direct yaw input + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_DIRCT_YAW, 0.0f); + +/** + * Direct thrust input + * + * @group UUV Attitude Control + */ +PARAM_DEFINE_FLOAT(UUV_DIRCT_THRUST, 0.0f);