diff --git a/ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix b/ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix index 6a1f1f63f9e..1b27ef16404 100644 --- a/ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix +++ b/ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix @@ -1,32 +1,40 @@ # Motor 1 -M: 2 +M: 3 S: 0 2 -4000 -4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 -4000 -4000 0 -4000 +4000 # Motor 2 -M: 2 +M: 3 S: 0 2 +4000 +4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 +4000 +4000 0 -4000 +4000 # Motor 3 -M: 2 +M: 3 S: 0 2 -4000 -4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 +4000 +4000 0 -4000 +4000 # Motor 4 -M: 2 +M: 3 S: 0 2 +4000 +4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 -4000 -4000 0 -4000 +4000 # Motor 5 -M: 2 +M: 3 S: 0 0 -4000 -4000 0 -4000 +4000 S: 0 1 +4000 +4000 0 -4000 +4000 +S: 0 5 -4000 -4000 0 -4000 +4000 # Motor 6 -M: 2 +M: 3 S: 0 0 -4000 -4000 0 -4000 +4000 S: 0 1 -4000 -4000 0 -4000 +4000 +S: 0 5 +4000 +4000 0 -4000 +4000 # Motor 7 -M: 2 +M: 3 S: 0 0 +4000 +4000 0 -4000 +4000 S: 0 1 +4000 +4000 0 -4000 +4000 +S: 0 5 +4000 +4000 0 -4000 +4000 # Motor 8 -M: 2 +M: 3 S: 0 0 +4000 +4000 0 -4000 +4000 S: 0 1 -4000 -4000 0 -4000 +4000 +S: 0 5 -4000 -4000 0 -4000 +4000 diff --git a/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix b/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix index 6a1f1f63f9e..0ef3b6501ce 100644 --- a/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix +++ b/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix @@ -1,32 +1,40 @@ # Motor 1 -M: 2 +M: 3 S: 0 2 -4000 -4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 +4000 +4000 0 -4000 +4000 # Motor 2 -M: 2 +M: 3 S: 0 2 +4000 +4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 -4000 -4000 0 -4000 +4000 # Motor 3 -M: 2 +M: 3 S: 0 2 -4000 -4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 -4000 -4000 0 -4000 +4000 # Motor 4 -M: 2 +M: 3 S: 0 2 +4000 +4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 +4000 +4000 0 -4000 +4000 # Motor 5 -M: 2 +M: 3 S: 0 0 -4000 -4000 0 -4000 +4000 S: 0 1 +4000 +4000 0 -4000 +4000 +S: 0 5 +4000 +4000 0 -4000 +4000 # Motor 6 -M: 2 +M: 3 S: 0 0 -4000 -4000 0 -4000 +4000 S: 0 1 -4000 -4000 0 -4000 +4000 +S: 0 5 -4000 -4000 0 -4000 +4000 # Motor 7 -M: 2 +M: 3 S: 0 0 +4000 +4000 0 -4000 +4000 S: 0 1 +4000 +4000 0 -4000 +4000 +S: 0 5 -4000 -4000 0 -4000 +4000 # Motor 8 -M: 2 +M: 3 S: 0 0 +4000 +4000 0 -4000 +4000 S: 0 1 -4000 -4000 0 -4000 +4000 +S: 0 5 +4000 +4000 0 -4000 +4000 diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index b65c7ee71d1..65495b3e1c7 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -89,6 +89,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 20a50e3bf5c..eac3a3fb827 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -84,6 +84,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 0905161880a..3592562acd1 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -86,6 +86,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 4bc52191d78..8167947868e 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -83,6 +83,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake index 7f76304e671..cd3f1ec6a76 100644 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -93,6 +93,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 4cb7672727e..3ad3f7fe8ea 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -91,6 +91,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/sitl/ctrlalloc.cmake b/boards/px4/sitl/ctrlalloc.cmake index 9d30f52c6ce..61c126f8fc0 100644 --- a/boards/px4/sitl/ctrlalloc.cmake +++ b/boards/px4/sitl/ctrlalloc.cmake @@ -57,6 +57,7 @@ px4_add_board( simulator temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 7ac353f20da..cccb9d94096 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -55,6 +55,7 @@ px4_add_board( simulator temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/sitl/nolockstep.cmake b/boards/px4/sitl/nolockstep.cmake index a8ddbbf8373..f7973049511 100644 --- a/boards/px4/sitl/nolockstep.cmake +++ b/boards/px4/sitl/nolockstep.cmake @@ -55,6 +55,7 @@ px4_add_board( simulator temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 879dd9941ba..cba4e8adfc2 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -55,6 +55,7 @@ px4_add_board( simulator temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index c70de702c6e..4c101974a40 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -54,6 +54,7 @@ px4_add_board( simulator temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/src/modules/uuv_pos_control/CMakeLists.txt b/src/modules/uuv_pos_control/CMakeLists.txt new file mode 100644 index 00000000000..f97dd0ea42d --- /dev/null +++ b/src/modules/uuv_pos_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_pos_control + MAIN uuv_pos_control + STACK_MAIN 1200 + COMPILE_FLAGS + SRCS + uuv_pos_control.cpp + ) diff --git a/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/modules/uuv_pos_control/uuv_pos_control.cpp new file mode 100644 index 00000000000..afa18d8e9fb --- /dev/null +++ b/src/modules/uuv_pos_control/uuv_pos_control.cpp @@ -0,0 +1,291 @@ +/**************************************************************************** + * + * 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 hippocampus control module and is designed for the + * BlueROV2. + * + * All the acknowledgments and credits for the fw wing app are reported in those files. + * + * @author Tim Hansen + * @author Daniel Duecker + */ + +#include "uuv_pos_control.hpp" + + + +/** + * UUV pos_controller app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int uuv_pos_control_main(int argc, char *argv[]); + + +UUVPOSControl::UUVPOSControl(): + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ +} + +UUVPOSControl::~UUVPOSControl() +{ + perf_free(_loop_perf); +} + +bool UUVPOSControl::init() +{ + if (!_vehicle_local_position_sub.registerCallback()) { + PX4_ERR("vehicle_pos callback registration failed!"); + return false; + } + + return true; +} + +void UUVPOSControl::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 UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z, + const float roll_des, const float pitch_des, const float yaw_des) +{ + //watch if inputs are not to high + vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {}; + vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + + vehicle_attitude_setpoint.roll_body = roll_des; + vehicle_attitude_setpoint.pitch_body = pitch_des; + vehicle_attitude_setpoint.yaw_body = yaw_des; + + vehicle_attitude_setpoint.thrust_body[0] = thrust_x; + vehicle_attitude_setpoint.thrust_body[1] = thrust_y; + vehicle_attitude_setpoint.thrust_body[2] = thrust_z; + + + _att_sp_pub.publish(vehicle_attitude_setpoint); +} + +void UUVPOSControl::pose_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos) +{ + //get current rotation of vehicle + Quatf q_att(vehicle_attitude.q); + Vector3f pos_des = Vector3f(x_pos_des, y_pos_des, z_pos_des); + + Vector3f p_control_output = Vector3f(_param_pose_gain_x.get() * (pos_des(0) - vlocal_pos.x) - _param_pose_gain_d_x.get() + * vlocal_pos.vx, + _param_pose_gain_y.get() * (pos_des(1) - vlocal_pos.y) - _param_pose_gain_d_y.get() * vlocal_pos.vy, + _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z) - _param_pose_gain_d_z.get() * vlocal_pos.vz); + + Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body) + + publish_attitude_setpoint(rotated_input(0), + rotated_input(1), + rotated_input(2), + roll_des, pitch_des, yaw_des); + +} + +void UUVPOSControl::stabilization_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos) +{ + //get current rotation of vehicle + Quatf q_att(vehicle_attitude.q); + Vector3f pos_des = Vector3f(0, 0, z_pos_des); + + Vector3f p_control_output = Vector3f(0, + 0, + _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z)); + //potential d controller missing + Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body) + + publish_attitude_setpoint(rotated_input(0) + x_pos_des, rotated_input(1) + y_pos_des, rotated_input(2), + roll_des, pitch_des, yaw_des); + +} + +void UUVPOSControl::Run() +{ + if (should_exit()) { + _vehicle_local_position_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + /* check vehicle control mode for changes to publication state */ + _vcontrol_mode_sub.update(&_vcontrol_mode); + + + /* update parameters from storage */ + parameters_update(); + + //vehicle_attitude_s attitude; + vehicle_local_position_s vlocal_pos; + + /* only run controller if local_pos changed */ + if (_vehicle_local_position_sub.update(&vlocal_pos)) { + + /* 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) { + + + _vehicle_attitude_sub.update(&_vehicle_attitude);//get current vehicle attitude + _pos_sp_triplet_sub.update(&_pos_setpoint); + + float roll_des, pitch_des, yaw_des, x_pos_des, y_pos_des, z_pos_des; + + + roll_des = 0; + pitch_des = 0; + yaw_des = _pos_setpoint.current.yaw; + + x_pos_des = _pos_setpoint.current.x; + y_pos_des = _pos_setpoint.current.y; + z_pos_des = _pos_setpoint.current.z; + + + //stabilization controller(keep pos and hold depth + angle) vs position controller(global + yaw) + if (_param_stabilization.get() == 0) { + pose_controller_6dof(x_pos_des, y_pos_des, z_pos_des, + roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos); + + } else { + stabilization_controller_6dof(x_pos_des, y_pos_des, z_pos_des, + roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos); + } + + + + } + } + + /* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */ + if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) { + // 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. + if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) { + /* manual/direct control */ + } + + } + + /* Only publish if any of the proper modes are enabled */ + if (_vcontrol_mode.flag_control_manual_enabled || + _vcontrol_mode.flag_control_attitude_enabled) { + } + + perf_end(_loop_perf); +} + +int UUVPOSControl::task_spawn(int argc, char *argv[]) +{ + UUVPOSControl *instance = new UUVPOSControl(); + + 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 UUVPOSControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + + +int UUVPOSControl::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_pos_control start +$ uuv_pos_control status +$ uuv_pos_control stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("uuv_pos_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start") + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int uuv_pos_control_main(int argc, char *argv[]) +{ + return UUVPOSControl::main(argc, argv); +} diff --git a/src/modules/uuv_pos_control/uuv_pos_control.hpp b/src/modules/uuv_pos_control/uuv_pos_control.hpp new file mode 100644 index 00000000000..b3e89c8129d --- /dev/null +++ b/src/modules/uuv_pos_control/uuv_pos_control.hpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * 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 hippocampus control module and is designed for the + * BlueROV2. + * + * All the acknowledgments and credits for the fw wing app are reported in those files. + * + * @author Tim Hansen + * @author Daniel Duecker + */ + +#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; + +using namespace time_literals; + +class UUVPOSControl: public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + UUVPOSControl(); + ~UUVPOSControl(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; /**< current vehicle attitude */ + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; /**< position setpoint */ + + uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; + + + //actuator_controls_s _actuators {}; /**< actuator control inputs */ + manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */ + vehicle_attitude_s _vehicle_attitude {}; /**< vehicle attitude */ + position_setpoint_triplet_s _pos_setpoint {}; /**< vehicle position setpoint */ + vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + DEFINE_PARAMETERS( + (ParamFloat) _param_pose_gain_x, + (ParamFloat) _param_pose_gain_y, + (ParamFloat) _param_pose_gain_z, + (ParamFloat) _param_pose_gain_d_x, + (ParamFloat) _param_pose_gain_d_y, + (ParamFloat) _param_pose_gain_d_z, + + (ParamInt) _param_input_mode, + (ParamInt) _param_stabilization, + (ParamInt) _param_skip_ctrl + ) + + void Run() override; + /** + * Update our local parameter cache. + */ + void parameters_update(bool force = false); + + /** + * Control Attitude + */ + void publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z, + const float roll_des, const float pitch_des, const float yaw_des); + void pose_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos); + void stabilization_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos); +}; diff --git a/src/modules/uuv_pos_control/uuv_pos_control_params.c b/src/modules/uuv_pos_control/uuv_pos_control_params.c new file mode 100644 index 00000000000..d3532edd43b --- /dev/null +++ b/src/modules/uuv_pos_control/uuv_pos_control_params.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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_pos_control_params.c + * + * Parameters defined by the position 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 Tim Hansen + * @author Daniel Duecker + */ + +/* + * Controller parameters, accessible via MAVLink + * + */ +/** + * Gain of P controller X + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_X_P, 1.0f); +/** + * Gain of P controller Y + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Y_P, 1.0f); +/** + * Gain of P controller Z + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Z_P, 1.0f); + +/** + * Gain of D controller X + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_X_D, 0.2f); +/** + * Gain of D controller Y + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Y_D, 0.2f); +/** + * Gain of D controller Z + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Z_D, 0.2f); + +/** + * Stabilization mode(1) or Position Control(0) + * + * @value 0 Position Control + * @value 1 Stabilization Mode + * @group UUV Position Control + */ +PARAM_DEFINE_INT32(UUV_STAB_MODE, 1);