Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

FlightTaskManualAltitude: Follow altitude limit given by the estimator #13958

Merged
merged 6 commits into from
Feb 3, 2020
Merged
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
FlightTaskManualAltitude: Rename _min_speed_down -> _max_speed_down a…
…s it is the maximum allowed downward speed
bresch committed Jan 17, 2020

Unverified

This user has not yet uploaded their public signing key.
commit b623122ae164512612508d741b04cae39bed09ce
Original file line number Diff line number Diff line change
@@ -64,7 +64,7 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s
_updateConstraintsFromEstimator();

_max_speed_up = _constraints.speed_up;
_min_speed_down = _constraints.speed_down;
_max_speed_down = _constraints.speed_down;

return ret;
}
@@ -246,7 +246,7 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
// below the maximum, preserving control loop vertical position error gain.
if (PX4_ISFINITE(_constraints.max_distance_to_ground)) {
_constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_constraints.max_distance_to_ground - _dist_to_bottom),
-_min_speed_down, _max_speed_up);
-_max_speed_down, _max_speed_up);

} else {
_constraints.speed_up = _max_speed_up;
@@ -259,10 +259,10 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
// set position setpoint to maximum distance to ground
_position_setpoint(2) = _position(2) + delta_distance_to_max;
// limit speed downwards to 0.7m/s
_constraints.speed_down = math::min(_min_speed_down, 0.7f);
_constraints.speed_down = math::min(_max_speed_down, 0.7f);

} else {
_constraints.speed_down = _min_speed_down;
_constraints.speed_down = _max_speed_down;

}
}
Original file line number Diff line number Diff line change
@@ -126,7 +126,7 @@ class FlightTaskManualAltitude : public FlightTaskManual
float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */
uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
float _max_speed_up = 10.0f;
float _min_speed_down = 1.0f;
float _max_speed_down = 1.0f;
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 */