dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

add support for unmanned underwater vehicles (#14079) 

* add support for unmanned underwater vehicles:
* airframe uuv_generic + uuv_hippocammpus including mav_type = 12 for submarines
* mixer for UUVs with X-shaped thruster setup similar to quadcopter
* add module  uuv_att_control for underwater robot attitude control
* add rc.uuv_defaults/apps for autostarting e.g. ekf2 and uuv_att_control app
This commit is contained in:
Daniel Duecker 2020-02-05 05:24:37 +01:00 committed by GitHub
parent a82428c5fd
commit 36f836be79
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
23 changed files with 1072 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ROMFS/px4fmu_common/init.d-posix/1020_hippocampus
View File

@ -1,8 +0,0 @@
#!/bin/sh
#
# @name Hippocampus UUV
#
sh /etc/init.d/rc.mc_defaults
set MIXER uuv_quad_x

23
ROMFS/px4fmu_common/init.d-posix/1020_uuv_generic Normal file
View File

@ -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

23
ROMFS/px4fmu_common/init.d-posix/1021_uuv_hippocampus Normal file
View File

@ -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

2
ROMFS/px4fmu_common/init.d/CMakeLists.txt
View File

@ -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

29
ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic Normal file
View File

@ -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

29
ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus Normal file
View File

@ -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 <daniel.duecker@tuhh.de>
#
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

4
ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
View File

@ -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
)

26
ROMFS/px4fmu_common/init.d/rc.uuv_apps Normal file
View File

@ -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

35
ROMFS/px4fmu_common/init.d/rc.uuv_defaults Normal file
View File

@ -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

20
ROMFS/px4fmu_common/init.d/rc.vehicle_setup
View File

@ -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).
#

1
ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt
View File

@ -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
)

25
ROMFS/px4fmu_common/mixers-sitl/uuv_x_sitl.main.mix Normal file
View File

@ -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

1
ROMFS/px4fmu_common/mixers/CMakeLists.txt
View File

@ -84,4 +84,5 @@ px4_add_romfs_files(
wingwing.main.mix
zmr250.main.mix
TF-AutoG2.main.mix
uuv_x.main.mix
)

22
ROMFS/px4fmu_common/mixers/uuv_quad_x.mix
View File

@ -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

25
ROMFS/px4fmu_common/mixers/uuv_x.main.mix Normal file
View File

@ -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

1
boards/px4/fmu-v4/default.cmake
View File

@ -82,6 +82,7 @@ px4_add_board(
temperature_compensation
vmount
vtol_att_control
uuv_att_control
SYSTEMCMDS
bl_update
config

2
boards/px4/sitl/default.cmake
View File

@ -48,6 +48,8 @@ px4_add_board(
temperature_compensation
vmount
vtol_att_control
uuv_att_control
SYSTEMCMDS
#config
#dumpfile

3
platforms/posix/cmake/sitl_target.cmake
View File

@ -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})

4
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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:

40
src/modules/uuv_att_control/CMakeLists.txt Normal file
View File

@ -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
)

452
src/modules/uuv_att_control/uuv_att_control.cpp Normal file
View File

@ -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 <daniel.duecker@tuhh.de>
* @author Philipp Hastedt <philipp.hastedt@tuhh.de>
* @author Tim Hansen <t.hansen@tuhh.de>
*/
#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);
}

192
src/modules/uuv_att_control/uuv_att_control.hpp Normal file
View File

@ -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 <daniel.duecker@tuhh.de>
* @author Philipp Hastedt <philipp.hastedt@tuhh.de>
* @author Tim Hansen <t.hansen@tuhh.de>
*/
#include <float.h>
#include <drivers/drv_hrt.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/perf/perf_counter.h>
#include <lib/pid/pid.h>
#include <matrix/math.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/uORB.h>
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Matrix3f;
using matrix::Vector3f;
using matrix::Dcmf;
using uORB::SubscriptionData;
class UUVAttitudeControl: public ModuleBase<UUVAttitudeControl>, 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<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<actuator_controls_s> _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_s> _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
perf_counter_t _loop_perf; /**< loop performance counter */
// estimator reset counters
uint8_t _pos_reset_counter{0}; // captures the number of times the estimator has reset the horizontal position
bool _debug{false}; /**< if set to true, print debug output */
int loop_counter = 0;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::UUV_ROLL_P>) _param_roll_p,
(ParamFloat<px4::params::UUV_ROLL_D>) _param_roll_d,
(ParamFloat<px4::params::UUV_PITCH_P>) _param_pitch_p,
(ParamFloat<px4::params::UUV_PITCH_D>) _param_pitch_d,
(ParamFloat<px4::params::UUV_YAW_P>) _param_yaw_p,
(ParamFloat<px4::params::UUV_YAW_D>) _param_yaw_d,
// control/input modes
(ParamInt<px4::params::UUV_INPUT_MODE>) _param_input_mode,
// direct access to inputs
(ParamFloat<px4::params::UUV_DIRCT_ROLL>) _param_direct_roll,
(ParamFloat<px4::params::UUV_DIRCT_PITCH>) _param_direct_pitch,
(ParamFloat<px4::params::UUV_DIRCT_YAW>) _param_direct_yaw,
(ParamFloat<px4::params::UUV_DIRCT_THRUST>) _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);
};

136
src/modules/uuv_att_control/uuv_att_control_params.c Normal file
View File

@ -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 <daniel.duecker@tuhh.de>
*/
/*
* 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);