FlightTaskManualAltitude.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   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
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file FlightManualAltitude.cpp
 */

#include "FlightTaskManualAltitude.hpp"
#include <float.h>
#include <mathlib/mathlib.h>

using namespace matrix;

bool FlightTaskManualAltitude::updateInitialize()
{
    bool ret = FlightTaskManual::updateInitialize();

    _sub_home_position.update();

    // in addition to manual require valid position and velocity in D-direction and valid yaw
    return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
}

bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_setpoint)
{
    bool ret = FlightTaskManual::activate(last_setpoint);
    _yaw_setpoint = NAN;
    _yawspeed_setpoint = 0.0f;
    _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity
    _position_setpoint(2) = _position(2);
    _velocity_setpoint(2) = 0.0f;
    _setDefaultConstraints();

    _constraints.tilt = math::radians(_param_mpc_man_tilt_max.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;
    }

    if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) {
        _constraints.max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max;

    } else {
        _constraints.max_distance_to_ground = INFINITY;
    }

    _max_speed_up = _constraints.speed_up;
    _min_speed_down = _constraints.speed_down;

    return ret;
}

void FlightTaskManualAltitude::_scaleSticks()
{
    // Use stick input with deadzone, exponential curve and first order lpf for yawspeed
    const float yawspeed_target = _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get());
    _yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);

    // Use sticks input with deadzone and exponential curve for vertical velocity
    const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
    _velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
}

float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
{
    const float den = math::max(_param_mpc_man_y_tau.get() + _deltatime, 0.001f);
    const float alpha = _deltatime / den;
    _yawspeed_filter_state = (1.f - alpha) * _yawspeed_filter_state + alpha * yawspeed_target;
    return _yawspeed_filter_state;
}

void FlightTaskManualAltitude::_updateAltitudeLock()
{
    // 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(_sticks_expo(2)) <= FLT_EPSILON;

    // Check if vehicle has stopped
    const bool stopped = (_param_mpc_hold_max_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _param_mpc_hold_max_z.get());

    // Manage transition between use of distance to ground and distance to local origin
    // when terrain hold behaviour has been selected.
    if (_param_mpc_alt_mode.get() == 2) {
        // Use horizontal speed as a transition criteria
        float spd_xy = Vector2f(_velocity).length();

        // Use presence of horizontal stick inputs as a transition criteria
        float stick_xy = Vector2f(&_sticks_expo(0)).length();
        bool stick_input = stick_xy > 0.001f;

        if (_terrain_hold) {
            bool too_fast = spd_xy > _param_mpc_hold_max_xy.get();

            if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) {
                // Stop using distance to ground
                _terrain_hold = false;
                _terrain_follow = false;

                // Adjust the setpoint to maintain the same height error to reduce control transients
                if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) {
                    _position_setpoint(2) = _position(2) + (_dist_to_ground_lock - _dist_to_bottom);

                } else {
                    _position_setpoint(2) = _position(2);
                }
            }

        } else {
            bool not_moving = spd_xy < 0.5f * _param_mpc_hold_max_xy.get();

            if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) {
                // Start using distance to ground
                _terrain_hold = true;
                _terrain_follow = true;

                // Adjust the setpoint to maintain the same height error to reduce control transients
                if (PX4_ISFINITE(_position_setpoint(2))) {
                    _dist_to_ground_lock = _dist_to_bottom + (_position_setpoint(2) - _position(2));
                }
            }
        }

    }

    if ((_param_mpc_alt_mode.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) {
        // terrain following
        _terrainFollowing(apply_brake, stopped);
        // respect maximum altitude
        _respectMaxAltitude();

    } else {
        // normal mode where height is dependent on local frame

        if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(2))) {
            // lock position
            _position_setpoint(2) = _position(2);

            // Ensure that minimum altitude is respected if
            // there is a distance sensor and distance to bottom is below minimum.
            if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < _constraints.min_distance_to_ground) {
                _terrainFollowing(apply_brake, stopped);

            } else {
                _dist_to_ground_lock = NAN;
            }

        } else if (PX4_ISFINITE(_position_setpoint(2)) && apply_brake) {
            // Position is locked but check if a reset event has happened.
            // We will shift the setpoints.
            if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counter) {
                _position_setpoint(2) = _position(2);
                _reset_counter = _sub_vehicle_local_position.get().z_reset_counter;
            }

        } else  {
            // user demands velocity change
            _position_setpoint(2) = NAN;
            // ensure that maximum altitude is respected
            _respectMaxAltitude();
        }
    }
}

void FlightTaskManualAltitude::_respectMinAltitude()
{
    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)
                    - (_constraints.min_distance_to_ground - _dist_to_bottom);
    }
}

void FlightTaskManualAltitude::_terrainFollowing(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.

        // 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::_respectMaxAltitude()
{
    if (PX4_ISFINITE(_dist_to_bottom)) {

        // if there is a valid maximum distance to ground, linearly increase speed limit with distance
        // 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);

        } else {
            _constraints.speed_up = _max_speed_up;
        }

        // if distance to bottom exceeded maximum distance, slowly approach maximum distance
        if (_dist_to_bottom >  _constraints.max_distance_to_ground) {
            // difference between current distance to ground and maximum distance to ground
            const float delta_distance_to_max = _dist_to_bottom - _constraints.max_distance_to_ground;
            // 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);

        } else {
            _constraints.speed_down = _min_speed_down;

        }
    }
}

void FlightTaskManualAltitude::_respectGroundSlowdown()
{
    float dist_to_ground = NAN;

    // if there is a valid distance to bottom or vertical distance to home
    if (PX4_ISFINITE(_dist_to_bottom)) {
        dist_to_ground = _dist_to_bottom;

    } else if (_sub_home_position.get().valid_alt) {
        dist_to_ground = -(_position(2) - _sub_home_position.get().z);
    }

    // limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
    if (PX4_ISFINITE(dist_to_ground)) {
        const float limit_down = math::gradual(dist_to_ground,
                               _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
                               _param_mpc_land_speed.get(), _constraints.speed_down);
        const float limit_up = math::gradual(dist_to_ground,
                             _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
                             _param_mpc_tko_speed.get(), _constraints.speed_up);
        _velocity_setpoint(2) = math::constrain(_velocity_setpoint(2), -limit_up, limit_down);
    }
}

void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v)
{
    float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
    Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
    v(0) = v_r(0);
    v(1) = v_r(1);
}

void FlightTaskManualAltitude::_updateHeadingSetpoints()
{
    if (_isYawInput()) {
        _unlockYaw();

    } else {
        _lockYaw();
    }
}

bool FlightTaskManualAltitude::_isYawInput()
{
    /*
     * A threshold larger than FLT_EPSILON is required because the
     * _yawspeed_setpoint comes from an IIR filter and takes too much
     * time to reach zero.
     */
    return fabsf(_yawspeed_setpoint) > 0.001f;
}

void FlightTaskManualAltitude::_unlockYaw()
{
    // no fixed heading when rotating around yaw by stick
    _yaw_setpoint = NAN;
}

void FlightTaskManualAltitude::_lockYaw()
{
    // hold the current heading when no more rotation commanded
    if (!PX4_ISFINITE(_yaw_setpoint)) {
        _yaw_setpoint = _yaw;
    }
}

void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
{
    // Only reset the yaw setpoint when the heading is locked
    if (PX4_ISFINITE(_yaw_setpoint)) {
        _yaw_setpoint += delta_psi;
    }
}

void FlightTaskManualAltitude::_updateSetpoints()
{
    _updateHeadingSetpoints(); // get yaw setpoint

    // 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));
    _rotateIntoHeadingFrame(sp);

    if (sp.length() > 1.0f) {
        sp.normalize();
    }

    _thrust_setpoint(0) = sp(0);
    _thrust_setpoint(1) = sp(1);
    _thrust_setpoint(2) = NAN;

    _updateAltitudeLock();
    _respectGroundSlowdown();
}

bool FlightTaskManualAltitude::_checkTakeoff()
{
    // stick is deflected above 65% throttle (_sticks(2) is in the range [-1,1])
    return _sticks(2) < -0.3f;
}

bool FlightTaskManualAltitude::update()
{
    _scaleSticks();
    _updateSetpoints();
    _constraints.want_takeoff = _checkTakeoff();

    return true;
}