From 51fe4351c600c50051cce8f207f7111aa7551a8c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 16 Feb 2024 09:58:55 +0100 Subject: [PATCH] StickTiltXY: Fix too high maximum tilt problem And add unit tests. --- .../tasks/Utility/CMakeLists.txt | 2 + .../tasks/Utility/StickTiltXY.cpp | 15 ++- .../tasks/Utility/StickTiltXY.hpp | 3 + .../tasks/Utility/StickTiltXYTest.cpp | 102 ++++++++++++++++++ .../tasks/Utility/Sticks.hpp | 2 +- .../multicopter_stabilized_mode_params.c | 4 +- 6 files changed, 123 insertions(+), 5 deletions(-) create mode 100644 src/modules/flight_mode_manager/tasks/Utility/StickTiltXYTest.cpp diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index 62b8e12179..911da11d26 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -40,3 +40,5 @@ px4_add_library(FlightTaskUtility target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_functional_gtest(SRC StickTiltXYTest.cpp LINKLIBS FlightTaskUtility) diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp index 49c9b2f83c..642c4ef8a1 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp @@ -40,7 +40,18 @@ using namespace matrix; StickTiltXY::StickTiltXY(ModuleParams *parent) : ModuleParams(parent) -{} +{ + updateParams(); +} + +void StickTiltXY::updateParams() +{ + ModuleParams::updateParams(); + // Consider maximum tilt but only between [0.02,3]g sideways acceleration -> ~[1,71]° tilt + // Constrain tilt already because tanf(90+°) will give negative result + const float maximum_tilt = math::radians(math::constrain(_param_mpc_man_tilt_max.get(), 0.f, 89.f)); + _maximum_acceleration = math::constrain(tanf(maximum_tilt), .02f, 3.f) * CONSTANTS_ONE_G; +} Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const float dt, const float yaw, const float yaw_setpoint) @@ -49,5 +60,5 @@ Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const flo _man_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get()); stick_xy = _man_input_filter.update(stick_xy); Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_setpoint); - return stick_xy * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G; + return stick_xy * _maximum_acceleration; } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp index 38f42ed384..f5caea8550 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.hpp @@ -63,6 +63,9 @@ public: matrix::Vector2f generateAccelerationSetpoints(matrix::Vector2f stick_xy, const float dt, const float yaw, const float yaw_setpoint); private: + void updateParams() override; + + float _maximum_acceleration{0.f}; AlphaFilter _man_input_filter; DEFINE_PARAMETERS( diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickTiltXYTest.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXYTest.cpp new file mode 100644 index 0000000000..0f7f88c3cc --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/StickTiltXYTest.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (C) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "StickTiltXY.hpp" + +#include + +using namespace matrix; + +TEST(StickTiltXYTest, AllZeroCase) +{ + StickTiltXY stick_tilt_xy{nullptr}; + Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(), 0.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f()); +} + +TEST(StickTiltXYTest, NormalRollPitchCases) +{ + // Disable autosaving parameters to avoid busy loop in param_set() + param_control_autosave(false); + + float value = 45.f; + param_set(param_find("MPC_MAN_TILT_MAX"), &value); + + StickTiltXY stick_tilt_xy{nullptr}; + // Pitch + Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 0.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G, 0.f)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(.5f, 0.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / 2.f, 0.f)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-.5f, 0.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / 2.f, 0.f)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, 0.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G, 0.f)); + // Roll + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, 1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(0.f, CONSTANTS_ONE_G)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, .5f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(0.f, CONSTANTS_ONE_G / 2.f)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, -.5f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(0.f, -CONSTANTS_ONE_G / 2.f)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(0.f, -1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(0.f, -CONSTANTS_ONE_G)); + // Roll & Pitch + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / M_SQRT2_F, CONSTANTS_ONE_G / M_SQRT2_F)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, -1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(CONSTANTS_ONE_G / M_SQRT2_F, -CONSTANTS_ONE_G / M_SQRT2_F)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, 1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / M_SQRT2_F, CONSTANTS_ONE_G / M_SQRT2_F)); + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(-1.f, -1.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(-CONSTANTS_ONE_G / M_SQRT2_F, -CONSTANTS_ONE_G / M_SQRT2_F)); +} + +TEST(StickTiltXYTest, 90degreeCase) +{ + // Disable autosaving parameters to avoid busy loop in param_set() + param_control_autosave(false); + + float value = 90.f; + param_set(param_find("MPC_MAN_TILT_MAX"), &value); + + StickTiltXY stick_tilt_xy{nullptr}; + // Pitch + // Zero input leads to zero output + Vector2f acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f()); + // Maximum input leads to the maximum of 3g sideways acceleration + acc_xy = stick_tilt_xy.generateAccelerationSetpoints(Vector2f(1.f, 0.f), 1.f, 0.f, 0.f); + EXPECT_EQ(acc_xy, Vector2f(3.f * CONSTANTS_ONE_G, 0.f)); +} diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp index be8b69e306..3ab13368e8 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp @@ -92,7 +92,7 @@ public: private: bool _input_available{false}; - matrix::Vector4f _positions; ///< unmodified manual stick inputs + matrix::Vector4f _positions; ///< unmodified manual stick inputs that usually move vehicle in x, y, z and yaw direction matrix::Vector4f _positions_expo; ///< modified manual sticks using expo function matrix::Vector _aux_positions; diff --git a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c index f478f9938e..481d9cce66 100644 --- a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c @@ -35,8 +35,8 @@ * Maximal tilt angle in Stabilized or Altitude mode * * @unit deg - * @min 0 - * @max 90 + * @min 1 + * @max 70 * @decimal 0 * @increment 1 * @group Multicopter Position Control