diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index 77134e7b2d..4935a8d248 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,19 +42,18 @@ using namespace matrix; void FlightTaskManualAltitude::_scaleSticks() { - /* Reuse same scaling as for stabilized */ + // reuse same scaling as for stabilized FlightTaskManualStabilized::_scaleSticks(); - /* Scale horizontal velocity with expo curve stick input*/ + // scale horizontal velocity with expo curve stick input const float vel_max_z = (_sticks(2) > 0.0f) ? _limits.speed_dn_max : _limits.speed_up_max; _velocity_setpoint(2) = vel_max_z * _sticks_expo(2); } void FlightTaskManualAltitude::_updateAltitudeLock() { - /* Depending on stick inputs and velocity, position is locked. - * If not locked, altitude setpoint is set to NAN. - */ + // Depending on stick inputs and velocity, position is locked. + // If not locked, altitude setpoint is set to NAN. // check if user wants to break const bool apply_brake = fabsf(_velocity_setpoint(2)) <= FLT_EPSILON; @@ -102,7 +101,7 @@ 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 height to ground. + // User wants to break and vehicle reached zero velocity. Lock height to ground. // lock position _position_setpoint(2) = _position(2); @@ -130,11 +129,11 @@ void FlightTaskManualAltitude::_updateSetpoints() _thrust_setpoint *= NAN; // Don't need thrust setpoint from Stabilized mode. - /* Thrust in xy are extracted directly from stick inputs. A magnitude of - * 1 means that maximum thrust along xy is required. A magnitude of 0 means no - * thrust along xy is required. The maximum thrust along xy depends on the thrust - * setpoint along z-direction, which is computed in PositionControl.cpp. - */ + // Thrust in xy are extracted directly from stick inputs. A magnitude of + // 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no + // thrust along xy is demanded. The maximum thrust along xy depends on the thrust + // setpoint along z-direction, which is computed in PositionControl.cpp. + Vector2f sp{_sticks(0), _sticks(1)}; _rotateIntoHeadingFrame(sp); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp index 27bcdc4f8e..430120d880 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp @@ -49,6 +49,9 @@ public: virtual ~FlightTaskManualAltitude() = default; protected: + void _updateSetpoints() override; /**< updates all setpoints */ + void _scaleSticks() override; /**< scales sticks to velocity in z */ + /** * Check and sets for position lock. * If sticks are at center position, the vehicle @@ -56,14 +59,6 @@ protected: */ void _updateAltitudeLock(); - void _respectMinAltitude(); - - /** - * - */ - void _updateSetpoints() override; /**< updates all setpoints */ - void _scaleSticks() override; /**< scales sticks to velocity in z */ - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized, (ParamFloat) MPC_HOLD_MAX_Z, (ParamFloat) SENS_FLOW_MINRNG, @@ -88,4 +83,11 @@ private: */ void _terrain_following(bool apply_brake, bool stopped); + /** + * Minimum Altitude during range sensor operation. + * If a range sensor is used for altitude estimates, for + * best operation a minimum altitude is required. The minimum + * altitude is only enforced during altitude lock. + */ + void _respectMinAltitude(); };