fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 09:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

flight mode manager: fix terrain hold

Browse Source
This commit is contained in:
alexklimaj
2023-11-07 12:00:14 -05:00
committed by Daniel Agar
parent 94d4dc85f8
commit de0910c767
6 changed files with 29 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+1 -3
View File
@@ -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
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+1 -2
View File
@@ -73,6 +73,7 @@ protected:
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<px4::params::MPC_HOLD_MAX_Z>) _param_mpc_hold_max_z,
@@ -117,8 +118,6 @@ private:
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 */