From 19bd7a91636c7347d3014fffe5c9aed5309987eb Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 25 Apr 2018 10:16:45 +0200 Subject: [PATCH] FlightTaskManualAltitude: terrain following --- .../tasks/FlightTaskManualAltitude.cpp | 74 ++++++++++++++++--- .../tasks/FlightTaskManualAltitude.hpp | 34 ++++++++- 2 files changed, 94 insertions(+), 14 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index 309f029004..77134e7b2d 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -56,24 +56,74 @@ void FlightTaskManualAltitude::_updateAltitudeLock() * If not locked, altitude setpoint is set to NAN. */ - /* handle position and altitude hold */ - const bool apply_brake_z = fabsf(_velocity_setpoint(2)) <= FLT_EPSILON; - const bool stopped_z = (MPC_HOLD_MAX_Z.get() < FLT_EPSILON || fabsf(_velocity(2)) < MPC_HOLD_MAX_Z.get()); + // check if user wants to break + const bool apply_brake = fabsf(_velocity_setpoint(2)) <= FLT_EPSILON; - if (apply_brake_z && stopped_z && !PX4_ISFINITE(_position_setpoint(2))) { - _position_setpoint(2) = _position(2); + // check if vehicle has stopped + const bool stopped = (MPC_HOLD_MAX_Z.get() < FLT_EPSILON || fabsf(_velocity(2)) < MPC_HOLD_MAX_Z.get()); - if ((PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < SENS_FLOW_MINRNG.get())) { - // if vehicle wants to keep altitude but is below minimum flow distance, - // increase altitude to minimum flow distance - _position_setpoint(2) = _position(2) - (SENS_FLOW_MINRNG.get() - _dist_to_bottom); + if (MPC_ALT_MODE.get() && PX4_ISFINITE(_dist_to_bottom)) { + // terrain following + _terrain_following(apply_brake, stopped); + + } else { + // altitude based on locale coordinate system + _dist_to_ground_lock = NAN; // reset boolean since not used + + if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(2))) { + // lock position + _position_setpoint(2) = _position(2); + // ensure that minimum altitude is respected + _respectMinAltitude(); + + } else if (!apply_brake) { + // user demands velocity change + _position_setpoint(2) = NAN; } - - } else if (!apply_brake_z) { - _position_setpoint(2) = NAN; } } +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(); + + // 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); + } +} + +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. + + // lock position + _position_setpoint(2) = _position(2); + // ensure that minimum altitude is respected + _respectMinAltitude(); + // lock distance to ground but adjust first for minimum altitude + _dist_to_ground_lock = _dist_to_bottom - (_position_setpoint(2) - _position(2)); + + } else if (apply_brake && PX4_ISFINITE(_dist_to_ground_lock)) { + // vehicle needs to follow terrain + + // difference between the current distance to ground and the desired distance to ground + const float delta_distance_to_ground = _dist_to_ground_lock - _dist_to_bottom; + // adjust position setpoint for the delta (note: NED frame) + _position_setpoint(2) = _position(2) - delta_distance_to_ground; + + } else { + // user demands velocity change in D-direction + _dist_to_ground_lock = _position_setpoint(2) = NAN; + } +} void FlightTaskManualAltitude::_updateSetpoints() { FlightTaskManualStabilized::_updateSetpoints(); // get yaw and thrust setpoints diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp index 2cfef2a64d..27bcdc4f8e 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp @@ -49,13 +49,43 @@ public: virtual ~FlightTaskManualAltitude() = default; protected: - void _updateAltitudeLock(); /**< checks for position lock */ + /** + * Check and sets for position lock. + * If sticks are at center position, the vehicle + * will exit velocity control and enter position control. + */ + 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 + (ParamFloat) SENS_FLOW_MINRNG, + (ParamInt) MPC_ALT_MODE ) +private: + /** + * Distance to ground during terrain following. + * If user does not demand velocity change in D-direction and the vehcile + * is in terrain-following mode, then height to ground will be locked to + * _dist_to_ground_lock. + */ + float _dist_to_ground_lock = NAN; + + /** + * Terrain following. + * During terrain following, the position setpoint is adjusted + * such that the distance to ground is kept constant. + * @param apply_brake is true if user wants to break + * @param stopped is true if vehicle has stopped moving in D-direction + */ + void _terrain_following(bool apply_brake, bool stopped); + };