StickTiltXY: Fix too high maximum tilt problem

And add unit tests.

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)

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;
 }

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<matrix::Vector2f> _man_input_filter;
 
 	DEFINE_PARAMETERS(

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 <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));
+}

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<float, 6> _aux_positions;

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
+ * @min 1
- * @max 90
+ * @max 70
  * @decimal 0
  * @increment 1
  * @group Multicopter Position Control