forked from Archive/PX4-Autopilot
flight mode manager: fix terrain hold
This commit is contained in:
parent
94d4dc85f8
commit
de0910c767
|
@ -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
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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 */
|
||||
};
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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 */
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue