diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index c965618fc8..716c37841b 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -55,6 +55,14 @@ bool FlightTaskAutoLine::update() bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position; bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position; + // Altitude above ground from home or if not present + // the negation of the current position + _alt_above_ground = -_position(2); + + if (_sub_home_position->get().valid_alt) { + _alt_above_ground = -_position(2) + _sub_home_position->get().z; + } + // 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state. if (follow_line && !follow_line_prev) { _reset(); @@ -380,9 +388,16 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints() // check sign const bool flying_upward = _destination(2) < _position(2); + + // limit vertical downwards speed (positive z) close to ground + // for now we use the altitude above home and assume that we want to land at same height as we took off + float vel_limit = math::gradual(_alt_above_ground, + _slow_land_alt2.get(), _slow_land_alt1.get(), + _land_speed.get(), _vel_max_down.get()); + // Speed at threshold is by default maximum speed. Threshold defines // the point in z at which vehicle slows down to reach target altitude. - float speed_sp = (flying_upward) ? _vel_max_up.get() : _vel_max_down.get(); + float speed_sp = (flying_upward) ? _vel_max_up.get() : vel_limit; // Target threshold defines the distance to target(2) at which // the vehicle starts to slow down to approach the target smoothly. @@ -493,3 +508,11 @@ float FlightTaskAutoLine::_getVelocityFromAngle(const float angle) // speed_close needs to be in between max and min return math::constrain(speed_close, min_cruise_speed, _mc_cruise_speed); } + +void FlightTaskAutoLine::updateParams() +{ + FlightTaskAuto::updateParams(); + + // make sure that alt1 is above alt2 + _slow_land_alt1.set(math::max(_slow_land_alt1.get(), _slow_land_alt2.get())); +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp index 575188d3b0..bf25649a2a 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp @@ -55,6 +55,7 @@ protected: matrix::Vector3f _destination{}; /**< Current target. Is not necessarily the same as triplet target. */ matrix::Vector3f _origin{}; /**< Previous waypoint. Is not necessarily the same as triplet previous. */ float _speed_at_target = 0.0f; /**< Desired velocity at target. */ + float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */ enum class State { offtrack, /**< Vehicle is more than cruise speed away from track */ @@ -74,7 +75,9 @@ protected: (ParamFloat) _acc_max_down, (ParamFloat) _cruise_speed_90, (ParamFloat) _nav_rad, - (ParamFloat) _mis_yaw_error + (ParamFloat) _mis_yaw_error, + (ParamFloat) _slow_land_alt1, + (ParamFloat) _slow_land_alt2 ) void _generateIdleSetpoints(); @@ -85,6 +88,7 @@ protected: void _generateSetpoints(); /**< Generate velocity and position setpoint for following line. */ void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */ void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */ + void updateParams() override; /**< See ModuleParam class */ private: float _getVelocityFromAngle(const float angle); /** Computes the speed at target depending on angle. */