flight mode manager: fix terrain hold

This commit is contained in:
alexklimaj 2023-11-01 16:42:53 +00:00 committed by Daniel Agar
parent 94d4dc85f8
commit de0910c767
6 changed files with 29 additions and 7 deletions

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

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 */

View File

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

View File

@ -67,4 +67,7 @@ protected:
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _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 */
};

View File

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

View File

@ -87,4 +87,6 @@ private:
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 */
};