From 26b41b95022063a5ec85e313d6b2b5aaf00f1651 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Wed, 1 Nov 2023 16:42:53 +0000 Subject: [PATCH] flight mode manager: fix terrain hold --- .../ManualAltitude/FlightTaskManualAltitude.cpp | 4 +--- .../ManualAltitude/FlightTaskManualAltitude.hpp | 3 +-- .../FlightTaskManualAltitudeSmoothVel.cpp | 12 +++++++++++- .../FlightTaskManualAltitudeSmoothVel.hpp | 3 +++ .../FlightTaskManualPositionSmoothVel.cpp | 12 +++++++++++- .../FlightTaskManualPositionSmoothVel.hpp | 2 ++ 6 files changed, 29 insertions(+), 7 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 90d5dccb1feb..3508d6b7701b 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -127,7 +127,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock() if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) { // Stop using distance to ground _terrain_hold = false; - _terrain_follow = false; // Adjust the setpoint to maintain the same height error to reduce control transients if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) { @@ -144,7 +143,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock() if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) { // Start using distance to ground _terrain_hold = true; - _terrain_follow = true; // Adjust the setpoint to maintain the same height error to reduce control transients if (PX4_ISFINITE(_position_setpoint(2))) { @@ -155,7 +153,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() } - if ((_param_mpc_alt_mode.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) { + if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) { // terrain following _terrainFollowing(apply_brake, stopped); // respect maximum altitude diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 0b9fbe72a3fb..be9eb278d38e 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -75,6 +75,7 @@ class FlightTaskManualAltitude : public FlightTask StickYaw _stick_yaw{this}; bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data + bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, (ParamFloat) _param_mpc_hold_max_z, @@ -121,8 +122,6 @@ class FlightTaskManualAltitude : public FlightTask void setGearAccordingToSwitch(); uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ - bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ - bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ float _min_distance_to_ground{(float)(-INFINITY)}; /**< min distance to ground constraint */ float _max_distance_to_ground{(float)INFINITY}; /**< max distance to ground constraint */ diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index dd7ee8225ef6..2f6128508e2d 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -106,5 +106,15 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState() _jerk_setpoint(2) = _smoothing.getCurrentJerk(); _acceleration_setpoint(2) = _smoothing.getCurrentAcceleration(); _velocity_setpoint(2) = _smoothing.getCurrentVelocity(); - _position_setpoint(2) = _smoothing.getCurrentPosition(); + + if (!_terrain_hold) { + if (_terrain_hold_previous) { + // Reset position setpoint to current position when switching from terrain hold to non-terrain hold + _smoothing.setCurrentPosition(_position(2)); + } + + _position_setpoint(2) = _smoothing.getCurrentPosition(); + } + + _terrain_hold_previous = _terrain_hold; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index 45e013fb7c82..468388c031b9 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -67,4 +67,7 @@ class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude (ParamFloat) _param_mpc_acc_up_max, (ParamFloat) _param_mpc_acc_down_max ) + +private: + bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ }; diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 14527ba712d2..a89283e14619 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -171,5 +171,15 @@ void FlightTaskManualPositionSmoothVel::_setOutputStateZ() _jerk_setpoint(2) = _smoothing_z.getCurrentJerk(); _acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration(); _velocity_setpoint(2) = _smoothing_z.getCurrentVelocity(); - _position_setpoint(2) = _smoothing_z.getCurrentPosition(); + + if (!_terrain_hold) { + if (_terrain_hold_previous) { + // Reset position setpoint to current position when switching from terrain hold to non-terrain hold + _smoothing_z.setCurrentPosition(_position(2)); + } + + _position_setpoint(2) = _smoothing_z.getCurrentPosition(); + } + + _terrain_hold_previous = _terrain_hold; } diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index aa82261e9d30..c8fbdf42c95a 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -87,4 +87,6 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction + + bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ };