StickTiltXY: Fix too high maximum tilt problem

And add unit tests.
This commit is contained in:
Matthias Grob 2024-02-16 09:58:55 +01:00
parent 8a75733511
commit 51fe4351c6
6 changed files with 123 additions and 5 deletions

View File

@ -40,3 +40,5 @@ px4_add_library(FlightTaskUtility
target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib) target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib)
target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
px4_add_functional_gtest(SRC StickTiltXYTest.cpp LINKLIBS FlightTaskUtility)

View File

@ -40,7 +40,18 @@ using namespace matrix;
StickTiltXY::StickTiltXY(ModuleParams *parent) : StickTiltXY::StickTiltXY(ModuleParams *parent) :
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, Vector2f StickTiltXY::generateAccelerationSetpoints(Vector2f stick_xy, const float dt, const float yaw,
const float yaw_setpoint) 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()); _man_input_filter.setParameters(dt, _param_mc_man_tilt_tau.get());
stick_xy = _man_input_filter.update(stick_xy); stick_xy = _man_input_filter.update(stick_xy);
Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_setpoint); 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;
} }

View File

@ -63,6 +63,9 @@ public:
matrix::Vector2f generateAccelerationSetpoints(matrix::Vector2f stick_xy, const float dt, const float yaw, matrix::Vector2f generateAccelerationSetpoints(matrix::Vector2f stick_xy, const float dt, const float yaw,
const float yaw_setpoint); const float yaw_setpoint);
private: private:
void updateParams() override;
float _maximum_acceleration{0.f};
AlphaFilter<matrix::Vector2f> _man_input_filter; AlphaFilter<matrix::Vector2f> _man_input_filter;
DEFINE_PARAMETERS( DEFINE_PARAMETERS(

View File

@ -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 <gtest/gtest.h>
#include "StickTiltXY.hpp"
#include <geo/geo.h>
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));
}

View File

@ -92,7 +92,7 @@ public:
private: private:
bool _input_available{false}; 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::Vector4f _positions_expo; ///< modified manual sticks using expo function
matrix::Vector<float, 6> _aux_positions; matrix::Vector<float, 6> _aux_positions;

View File

@ -35,8 +35,8 @@
* Maximal tilt angle in Stabilized or Altitude mode * Maximal tilt angle in Stabilized or Altitude mode
* *
* @unit deg * @unit deg
* @min 0 * @min 1
* @max 90 * @max 70
* @decimal 0 * @decimal 0
* @increment 1 * @increment 1
* @group Multicopter Position Control * @group Multicopter Position Control