fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlighttaskManualAlititude: replace SENS_FLOW_MINRNG with hagl_min

Browse Source
This commit is contained in:
Dennis Mannhart 2018-06-25 07:48:19 +02:00 committed by Lorenz Meier
parent 4af9d79986
commit c907e7a9dc
3 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/lib/FlightTasks/tasks/FlightTask.cpp
View File

@ -96,12 +96,13 @@ bool FlightTask::_evaluateVehicleLocalPosition()
// distance to bottom
_dist_to_bottom = NAN;
if (_sub_vehicle_local_position->get().dist_bottom_valid) {
if (_sub_vehicle_local_position->get().dist_bottom_valid
&& PX4_ISFINITE(_sub_vehicle_local_position->get().dist_bottom)) {
_dist_to_bottom = _sub_vehicle_local_position->get().dist_bottom;
}
// estimator specified vehicle limits
// We don't check here if states are valid or not.
// Validity checks are done in the sub-classes.

19
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
View File

@ -54,7 +54,14 @@ bool FlightTaskManualAltitude::activate()
_position_setpoint(2) = _position(2);
_velocity_setpoint(2) = 0.0f;
_setDefaultConstraints();
_constraints.min_distance_to_ground = SENS_FLOW_MINRNG.get();
if (PX4_ISFINITE(_sub_vehicle_local_position->get().hagl_min)) {
_constraints.min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;
} else {
_constraints.min_distance_to_ground = -INFINITY;
}
return ret;
}
@ -92,7 +99,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
// Ensure that minimum altitude is respected if
// there is a distance sensor and distance to bottome is below minimum.
if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < SENS_FLOW_MINRNG.get()) {
if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _constraints.min_distance_to_ground) {
_terrain_following(apply_brake, stopped);
} else {
@ -116,21 +123,19 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
void FlightTaskManualAltitude::_respectMinAltitude()
{
const bool respectAlt = _sub_vehicle_local_position->get().limit_hagl
&& PX4_ISFINITE(_dist_to_bottom)
&& _dist_to_bottom < SENS_FLOW_MINRNG.get();
const bool respectAlt = PX4_ISFINITE(_dist_to_bottom)
&& _dist_to_bottom < _constraints.min_distance_to_ground;
// Height above ground needs to be limited (flow / range-finder)
if (respectAlt) {
// increase altitude to minimum flow distance
_position_setpoint(2) = _position(2)
- (SENS_FLOW_MINRNG.get() - _dist_to_bottom);
- (_constraints.min_distance_to_ground - _dist_to_bottom);
}
}
void FlightTaskManualAltitude::_terrain_following(bool apply_brake, bool stopped)
{
if (apply_brake && stopped && !PX4_ISFINITE(_dist_to_ground_lock)) {
// User wants to break and vehicle reached zero velocity. Lock height to ground.

1
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
View File

@ -62,7 +62,6 @@ protected:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized,
(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z,
(ParamFloat<px4::params::SENS_FLOW_MINRNG>) SENS_FLOW_MINRNG,
(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE
)
private: