|
|
|
@ -64,7 +64,7 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s
@@ -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()
@@ -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()
@@ -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; |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|