From 49a1ed4683ce798084fcb94801eaba845dcbddff Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Wed, 1 Nov 2023 16:42:53 +0000 Subject: [PATCH 1/2] 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 ab90d10e1293..3173a0c7d241 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -124,7 +124,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)) { @@ -141,7 +140,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))) { @@ -152,7 +150,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 bf7ec2dc49d7..05e7b74b7418 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -73,6 +73,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, @@ -117,8 +118,6 @@ class FlightTaskManualAltitude : public FlightTask bool _updateYawCorrection(); 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 */ }; From 5cf930f9188a79a6d2933db1b87589b59b422f23 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Mon, 6 Nov 2023 12:33:09 -0700 Subject: [PATCH 2/2] mc_pos_control: MPC_ALT_MODE make terrain hold default --- src/modules/mc_pos_control/multicopter_altitude_mode_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c index ecebaa9e01f9..3f654fade71a 100644 --- a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c @@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_Y_TAU, 0.08f); * @value 2 Terrain hold * @group Multicopter Position Control */ -PARAM_DEFINE_INT32(MPC_ALT_MODE, 0); +PARAM_DEFINE_INT32(MPC_ALT_MODE, 2); /** * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check)