From 54ce9813c8e215e119bcf1b980b0430445d56e42 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 14 Aug 2023 16:42:33 +0200 Subject: [PATCH] FlightModeManager: Add task for position slow mode --- msg/CMakeLists.txt | 1 + msg/VelocityLimits.msg | 8 + .../flight_mode_manager/CMakeLists.txt | 1 + .../flight_mode_manager/FlightModeManager.cpp | 7 + .../ManualAccelerationSlow/CMakeLists.txt | 39 +++++ .../FlightTaskManualAccelerationSlow.cpp | 138 +++++++++++++++ .../FlightTaskManualAccelerationSlow.hpp | 88 ++++++++++ .../flight_task_acceleration_slow_params.c | 158 ++++++++++++++++++ 8 files changed, 440 insertions(+) create mode 100644 msg/VelocityLimits.msg create mode 100644 src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/CMakeLists.txt create mode 100644 src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp create mode 100644 src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp create mode 100644 src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 286607edb0..899698d476 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -234,6 +234,7 @@ set(msg_files VehicleTorqueSetpoint.msg VehicleTrajectoryBezier.msg VehicleTrajectoryWaypoint.msg + VelocityLimits.msg VtolVehicleStatus.msg WheelEncoders.msg Wind.msg diff --git a/msg/VelocityLimits.msg b/msg/VelocityLimits.msg new file mode 100644 index 0000000000..9ab5115abc --- /dev/null +++ b/msg/VelocityLimits.msg @@ -0,0 +1,8 @@ +# Velocity and yaw rate limits for a multicopter position slow mode only + +uint64 timestamp # time since system start (microseconds) + +# absolute speeds, NAN means use default limit +float32 horizontal_velocity # [m/s] +float32 vertical_velocity # [m/s] +float32 yaw_rate # [rad/s] diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index bee40bbc66..81ff6a7a33 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -43,6 +43,7 @@ list(APPEND flight_tasks_all Descend Failsafe ManualAcceleration + ManualAccelerationSlow ManualAltitude ManualAltitudeSmoothVel ManualPosition diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 9b5bec7a90..3d70726073 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -196,6 +196,13 @@ void FlightModeManager::start_flight_task() } } + // position slow mode + if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW) { + found_some_task = true; + FlightTaskError error = switchTask(FlightTaskIndex::ManualAccelerationSlow); + task_failure = error != FlightTaskError::NoError; + } + // Manual position control if ((_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) || task_failure) { found_some_task = true; diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/CMakeLists.txt new file mode 100644 index 0000000000..b4107f21d4 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2023 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_library(FlightTaskManualAccelerationSlow + FlightTaskManualAccelerationSlow.cpp +) +target_include_directories(FlightTaskManualAccelerationSlow PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(FlightTaskManualAccelerationSlow PUBLIC FlightTaskManualAcceleration FlightTaskUtility) diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp new file mode 100644 index 0000000000..8679573800 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 FlightTaskManualAccelerationSlow.cpp + */ + +#include "FlightTaskManualAccelerationSlow.hpp" +#include + +using namespace time_literals; + +bool FlightTaskManualAccelerationSlow::update() +{ + // Used to apply a configured default slowdown if neither MAVLink nor remote knob commands limits + bool velocity_horizontal_limited = false; + bool velocity_vertical_limited = false; + bool yaw_rate_limited = false; + + // Limits which can only slow down from the nominal configuration we initialize with here + // This is ensured by the executing classes + float velocity_horizontal = _param_mpc_vel_manual.get(); + float velocity_up = _param_mpc_z_vel_max_up.get(); + float velocity_down = _param_mpc_z_vel_max_dn.get(); + float yaw_rate = math::radians(_param_mpc_man_y_max.get()); + + // MAVLink commanded limits + if (_velocity_limits_sub.update(&_velocity_limits)) { + _velocity_limits_received_before = true; + } + + if (_velocity_limits_received_before) { + // message received once since mode was started + if (PX4_ISFINITE(_velocity_limits.horizontal_velocity)) { + velocity_horizontal = fmaxf(_velocity_limits.horizontal_velocity, _param_posslow_min_hvel.get()); + velocity_horizontal_limited = true; + } + + if (PX4_ISFINITE(_velocity_limits.vertical_velocity)) { + velocity_up = velocity_down = fmaxf(_velocity_limits.vertical_velocity, _param_posslow_min_vvel.get()); + velocity_vertical_limited = true; + } + + if (PX4_ISFINITE(_velocity_limits.yaw_rate)) { + yaw_rate = fmaxf(_velocity_limits.yaw_rate, math::radians(_param_posslow_min_yawr.get())); + yaw_rate_limited = true; + } + } + + // Remote knob commanded limits + if (_param_posslow_map_hvel.get() != 0) { + const float min_horizontal_velocity_scale = _param_posslow_min_hvel.get() / fmaxf(velocity_horizontal, FLT_EPSILON); + const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_posslow_map_hvel.get()); + const float aux_based_scale = + math::interpolate(aux_input, -1.f, 1.f, min_horizontal_velocity_scale, 1.f); + velocity_horizontal *= aux_based_scale; + velocity_horizontal_limited = true; + } + + if (_param_posslow_map_vvel.get() != 0) { + const float min_up_speed_scale = _param_posslow_min_vvel.get() / fmaxf(velocity_up, FLT_EPSILON); + const float min_down_speed_scale = _param_posslow_min_vvel.get() / fmaxf(velocity_down, FLT_EPSILON); + const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_posslow_map_vvel.get()); + const float up_aux_based_scale = + math::interpolate(aux_input, -1.f, 1.f, min_up_speed_scale, 1.f); + const float down_aux_based_scale = + math::interpolate(aux_input, -1.f, 1.f, min_down_speed_scale, 1.f); + velocity_up *= up_aux_based_scale; + velocity_down *= down_aux_based_scale; + velocity_vertical_limited = true; + } + + if (_param_posslow_map_yawr.get() != 0) { + const float min_yaw_rate_scale = math::radians(_param_posslow_min_yawr.get()) / fmaxf(yaw_rate, FLT_EPSILON); + const float aux_input = getInputFromSanitizedAuxParameterIndex(_param_posslow_map_yawr.get()); + const float aux_based_scale = + math::interpolate(aux_input, -1.f, 1.f, min_yaw_rate_scale, 1.f); + yaw_rate *= aux_based_scale; + yaw_rate_limited = true; + } + + // No input from remote and MAVLink -> use default slow mode limits + if (!velocity_horizontal_limited) { + velocity_horizontal = _param_posslow_def_hvel.get(); + } + + if (!velocity_vertical_limited) { + velocity_up = velocity_down = _param_posslow_def_vvel.get(); + } + + if (!yaw_rate_limited) { + yaw_rate = math::radians(_param_posslow_def_yawr.get()); + } + + // Interface to set resulting velocity limits + FlightTaskManualAcceleration::_stick_acceleration_xy.setVelocityConstraint(velocity_horizontal); + FlightTaskManualAltitude::_velocity_constraint_up = velocity_up; + FlightTaskManualAltitude::_velocity_constraint_down = velocity_down; + FlightTaskManualAcceleration::_stick_yaw.setYawspeedConstraint(yaw_rate); + + return FlightTaskManualAcceleration::update(); +} + +float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(int parameter_value) +{ + const int sanitized_index = math::constrain(parameter_value - 1, 0, 5); + return _sticks.getAux()(sanitized_index); +} diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp new file mode 100644 index 0000000000..8a9c541a37 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 FlightTaskManualAccelerationSlow.hpp + * + * Flight task for manual position mode with knobs for slower velocity and acceleration. + * + */ + +#pragma once + +#include "FlightTaskManualAcceleration.hpp" +#include "StickAccelerationXY.hpp" + +#include +#include +#include +#include +#include + +class FlightTaskManualAccelerationSlow : public FlightTaskManualAcceleration +{ +public: + FlightTaskManualAccelerationSlow() = default; + ~FlightTaskManualAccelerationSlow() = default; + bool update() override; + +private: + + /** + * Get the input from a sanitized parameter aux index + * @param parameter_value value of the parameter that specifies the AUX channel index to use + * @return input from that AUX channel [-1,1] + */ + float getInputFromSanitizedAuxParameterIndex(int parameter_value); + + bool _velocity_limits_received_before{false}; + + uORB::Subscription _velocity_limits_sub{ORB_ID(velocity_limits)}; + velocity_limits_s _velocity_limits{}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAcceleration, + (ParamInt) _param_posslow_map_hvel, + (ParamInt) _param_posslow_map_vvel, + (ParamInt) _param_posslow_map_yawr, + (ParamFloat) _param_posslow_min_hvel, + (ParamFloat) _param_posslow_min_vvel, + (ParamFloat) _param_posslow_min_yawr, + (ParamFloat) _param_posslow_def_hvel, + (ParamFloat) _param_posslow_def_vvel, + (ParamFloat) _param_posslow_def_yawr, + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_z_vel_max_up, + (ParamFloat) _param_mpc_z_vel_max_dn, + (ParamFloat) _param_mpc_man_y_max + ) +}; diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c new file mode 100644 index 0000000000..b2456f1df8 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + +/** + * Manual input mapped to scale horizontal velocity in position slow mode + * + * @value 0 No rescaling + * @value 1 AUX1 + * @value 2 AUX2 + * @value 3 AUX3 + * @value 4 AUX4 + * @value 5 AUX5 + * @value 6 AUX6 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_INT32(POSSLOW_MAP_HVEL, 0); + +/** + * Manual input mapped to scale vertical velocity in position slow mode + * + * @value 0 No rescaling + * @value 1 AUX1 + * @value 2 AUX2 + * @value 3 AUX3 + * @value 4 AUX4 + * @value 5 AUX5 + * @value 6 AUX6 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_INT32(POSSLOW_MAP_VVEL, 0); + +/** + * Manual input mapped to scale yaw rate in position slow mode + * + * @value 0 No rescaling + * @value 1 AUX1 + * @value 2 AUX2 + * @value 3 AUX3 + * @value 4 AUX4 + * @value 5 AUX5 + * @value 6 AUX6 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_INT32(POSSLOW_MAP_YAWR, 0); + +/** + * Horizontal velocity lower limit + * + * The lowest input maps and is clamped to this velocity. + * + * @unit m/s + * @min 0.1 + * @increment 0.1 + * @decimal 2 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_FLOAT(POSSLOW_MIN_HVEL, .3f); + +/** + * Vertical velocity lower limit + * + * The lowest input maps and is clamped to this velocity. + * + * @unit m/s + * @min 0.1 + * @increment 0.1 + * @decimal 2 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_FLOAT(POSSLOW_MIN_VVEL, .3f); + +/** + * Yaw rate lower limit + * + * The lowest input maps and is clamped to this rate. + * + * @unit deg/s + * @min 1 + * @increment 0.1 + * @decimal 0 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_FLOAT(POSSLOW_MIN_YAWR, 3.f); + +/** + * Default horizontal velocity limit + * + * This value is used in slow mode if + * no aux channel is mapped and + * no limit is commanded through MAVLink. + * + * @unit m/s + * @min 0.1 + * @increment 0.1 + * @decimal 2 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_FLOAT(POSSLOW_DEF_HVEL, 3.f); + +/** + * Default vertical velocity limit + * + * This value is used in slow mode if + * no aux channel is mapped and + * no limit is commanded through MAVLink. + * + * @unit m/s + * @min 0.1 + * @increment 0.1 + * @decimal 2 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_FLOAT(POSSLOW_DEF_VVEL, 1.f); + +/** + * Default yaw rate limit + * + * This value is used in slow mode if + * no aux channel is mapped and + * no limit is commanded through MAVLink. + * + * @unit deg/s + * @min 1 + * @increment 0.1 + * @decimal 0 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_FLOAT(POSSLOW_DEF_YAWR, 45.f);