From 5136957f498ec72f4f9d95f3c2202e26da33cca8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 9 Mar 2022 19:29:17 +0100 Subject: [PATCH 1/4] FlightTaskOrbit: remap stick input when vehicle faces tangential --- .../tasks/Orbit/FlightTaskOrbit.cpp | 37 +++++++++++++++---- .../tasks/Orbit/FlightTaskOrbit.hpp | 2 + 2 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 7c84eb986ba2..ffe09f943216 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -190,13 +190,7 @@ bool FlightTaskOrbit::update() { bool ret = true; _updateTrajectoryBoundaries(); - - // stick input adjusts parameters within a fixed time frame - float radius = _orbit_radius - _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_xy_cruise.get(); - float velocity = _orbit_velocity - _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_acc_hor.get(); - _sanitizeParams(radius, velocity); - _orbit_radius = radius; - _orbit_velocity = velocity; + _adjustParametersByStick(); if (_is_position_on_circle()) { if (_in_circle_approach) { @@ -215,7 +209,7 @@ bool FlightTaskOrbit::update() // update altitude ret = ret && FlightTaskManualAltitudeSmoothVel::update(); - // this generates x / y setpoints + // this generates x, y and yaw setpoints _generate_circle_setpoints(); _generate_circle_yaw_setpoints(); } @@ -262,6 +256,33 @@ bool FlightTaskOrbit::_is_position_on_circle() const } +void FlightTaskOrbit::_adjustParametersByStick() +{ + float radius = _orbit_radius; + float velocity = _orbit_velocity; + + switch (_yaw_behaviour) { + case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE: + radius -= _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_xy_cruise.get(); + velocity += _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_acc_hor.get(); + break; + + case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING: + case orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED: + case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED: + case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER: + default: + // stick input adjusts parameters within a fixed time frame + radius -= _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_xy_cruise.get(); + velocity -= _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_acc_hor.get(); + break; + } + + _sanitizeParams(radius, velocity); + _orbit_radius = radius; + _orbit_velocity = velocity; +} + void FlightTaskOrbit::_generate_circle_approach_setpoints() { const Vector2f center2d = Vector2f(_center); diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 9ebd760dde04..6bc54bfa5b4e 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -106,6 +106,8 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel */ bool _is_position_on_circle() const; + /** Adjusts radius and speed according to stick input */ + void _adjustParametersByStick(); /** generates setpoints to smoothly reach the closest point on the circle when starting from far away */ void _generate_circle_approach_setpoints(); /** generates xy setpoints to make the vehicle orbit */ From f8a5ed536eea9ccccaafc2513476e695a114d8e3 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 9 Mar 2022 20:20:13 +0100 Subject: [PATCH 2/4] Functions: add sign from boolean function with unit tests --- src/lib/mathlib/math/Functions.hpp | 11 +++++++++++ src/lib/mathlib/math/FunctionsTest.cpp | 9 +++++++++ 2 files changed, 20 insertions(+) diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 6602a71c7baf..13de8e8ad7bc 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -54,6 +54,17 @@ int signNoZero(T val) return (T(0) <= val) - (val < T(0)); } +/** + * Sign function based on a boolean + * + * @param[in] positive Truth value to take the sign from + * @return 1 if positive is true, -1 if positive is false + */ +inline int signFromBool(bool positive) +{ + return positive ? 1 : -1; +} + template T sq(T val) { diff --git a/src/lib/mathlib/math/FunctionsTest.cpp b/src/lib/mathlib/math/FunctionsTest.cpp index 49fd5dee6bae..48ac63fcec3a 100644 --- a/src/lib/mathlib/math/FunctionsTest.cpp +++ b/src/lib/mathlib/math/FunctionsTest.cpp @@ -47,6 +47,15 @@ TEST(FunctionsTest, signNoZero) EXPECT_FLOAT_EQ(signNoZero(123.456f), 1.f); } +TEST(FunctionsTest, signFromBool) +{ + EXPECT_EQ(signFromBool(true), 1); + EXPECT_EQ(signFromBool(false), -1); + EXPECT_EQ(signFromBool(100), 1); + EXPECT_EQ(signFromBool(-100), 1); + EXPECT_EQ(signFromBool(0), -1); +} + TEST(FunctionsTest, expo) { // input value limits From 7ebe36bd594470144cc32f1874dd35798aaf3b1e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 9 Mar 2022 20:22:51 +0100 Subject: [PATCH 3/4] FlightTaskOrbit: fix tangential yaw stick input for rotation direction changes --- .../tasks/Orbit/FlightTaskOrbit.cpp | 10 ++++++---- .../tasks/Orbit/FlightTaskOrbit.hpp | 1 + 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index ffe09f943216..e03860515831 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -68,7 +68,8 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) new_absolute_velocity = command.param2; } - float new_velocity = (new_is_clockwise ? 1.f : -1.f) * new_absolute_velocity; + float new_velocity = signFromBool(new_is_clockwise) * new_absolute_velocity; + _started_clockwise = new_is_clockwise; _sanitizeParams(new_radius, new_velocity); _orbit_radius = new_radius; _orbit_velocity = new_velocity; @@ -263,8 +264,8 @@ void FlightTaskOrbit::_adjustParametersByStick() switch (_yaw_behaviour) { case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE: - radius -= _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_xy_cruise.get(); - velocity += _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_acc_hor.get(); + radius -= signFromBool(_started_clockwise) * _sticks.getPositionExpo()(1) * _deltatime * _param_mpc_xy_cruise.get(); + velocity += signFromBool(_started_clockwise) * _sticks.getPositionExpo()(0) * _deltatime * _param_mpc_acc_hor.get(); break; case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING: @@ -339,7 +340,8 @@ void FlightTaskOrbit::_generate_circle_yaw_setpoints() break; case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE: - _yaw_setpoint = atan2f(sign(_orbit_velocity) * center_to_position(0), -sign(_orbit_velocity) * center_to_position(1)); + _yaw_setpoint = atan2f(signFromBool(_started_clockwise) * center_to_position(0), + -signFromBool(_started_clockwise) * center_to_position(1)); _yawspeed_setpoint = _orbit_velocity / _orbit_radius; break; diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 6bc54bfa5b4e..c4a3a7049e77 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -124,6 +124,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel /** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */ int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER; + bool _started_clockwise{true}; float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */ SlewRateYaw _slew_rate_yaw; From 0e21d1e44ec037c97fa1bdecad7c768ea9eb3cb4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 21 Mar 2022 18:41:22 +0100 Subject: [PATCH 4/4] matrix helper: remove needless comment line --- src/lib/matrix/matrix/helper_functions.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/lib/matrix/matrix/helper_functions.hpp b/src/lib/matrix/matrix/helper_functions.hpp index fa4e5c84ca9a..b6a0e3260480 100644 --- a/src/lib/matrix/matrix/helper_functions.hpp +++ b/src/lib/matrix/matrix/helper_functions.hpp @@ -143,7 +143,6 @@ Type unwrap(const Type last_x, const Type new_x, const Type low, const Type high * * @param[in] last_angle Last unwrapped angle [rad] * @param[in] new_angle New angle in [-pi, pi] [rad] - * @param * @return New unwrapped angle [rad] */ template