FlightTaskAutoLine.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 FlightAutoLine.cpp
 */

#include "FlightTaskAutoLine.hpp"
#include <mathlib/mathlib.h>

using namespace matrix;

static constexpr float SIGMA_NORM       =   0.001f;

void FlightTaskAutoLine::_generateSetpoints()
{
    if (!PX4_ISFINITE(_yaw_setpoint)) {
        // no valid heading -> set heading along track
        _generateHeadingAlongTrack();
    }

    if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
        // Wait for the yaw setpoint to be aligned
        if (!_position_locked) {
            _velocity_setpoint.setAll(0.f);
            _position_setpoint = _position;
            _position_locked = true;
        }

    } else {
        _position_locked = false;
        _generateAltitudeSetpoints();
        _generateXYsetpoints();
    }
}

void FlightTaskAutoLine::_setSpeedAtTarget()
{
    // angle goes from 0 to 2 with 0 = large angle, 2 = small angle:   0 = PI ; 2 = PI*0
    float angle = 2.0f; // minimum angle
    _speed_at_target = 0.0f;

    if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
        (Vector2f(&(_target - _prev_wp)(0)).length() > _target_acceptance_radius)) {
        // angle = cosf(x) + 1.0f
        angle =
            Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
            * Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
            + 1.0f;
        _speed_at_target = math::expontialFromLimits(angle, 0.0f, _param_mpc_cruise_90.get(), _mc_cruise_speed);
    }
}


void FlightTaskAutoLine::_generateHeadingAlongTrack()
{
    Vector2f prev_to_dest(_target - _prev_wp);
    _compute_heading_from_2D_vector(_yaw_setpoint, prev_to_dest);

}

void FlightTaskAutoLine::_generateXYsetpoints()
{
    _setSpeedAtTarget();
    Vector2f pos_sp_to_dest(_target - _position_setpoint);
    const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _param_nav_mc_alt_rad.get();

    if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _target_acceptance_radius) ||
        (!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) {

        // Vehicle reached target in xy and no passing required. Lock position
        _position_setpoint(0) = _target(0);
        _position_setpoint(1) = _target(1);
        _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;

    } else {
        // Vehicle needs to pass waypoint
        // Ensure that vehicle never gets stuck because of too low target-speed
        _speed_at_target = math::max(_speed_at_target, 0.5f);

        // Get various path specific vectors. */
        Vector2f u_prev_to_dest = Vector2f(_target - _prev_wp).unit_or_zero();
        Vector2f prev_to_pos(_position - _prev_wp);
        Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
        Vector2f closest_to_dest(_target - _position);
        Vector2f prev_to_dest(_target - _prev_wp);
        float speed_sp_track = _mc_cruise_speed;
        float speed_sp_prev_track = math::max(Vector2f(_velocity_setpoint) * u_prev_to_dest, 0.0f);

        // Distance to target when brake should occur. The assumption is made that
        // 1.5 * cruising speed is enough to break.
        float target_threshold = 1.5f * _mc_cruise_speed;
        float speed_threshold = _mc_cruise_speed;
        const float threshold_max = target_threshold;

        if (target_threshold > 0.5f * prev_to_dest.length()) {
            // Target threshold cannot be more than distance from previous to target
            target_threshold = 0.5f * prev_to_dest.length();
        }

        // Compute maximum speed at target threshold */
        if (threshold_max > _target_acceptance_radius) {
            float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _target_acceptance_radius);
            speed_threshold = m * (target_threshold - _target_acceptance_radius) + _speed_at_target; // speed at transition
        }

        // Either accelerate or decelerate
        if (closest_to_dest.length() < target_threshold) {

            // Vehicle is close to destination. Start to decelerate

            if (!has_reached_altitude) {
                // Altitude is not reached yet. Vehicle has to stop first before proceeding
                _speed_at_target = 0.0f;
            }

            float acceptance_radius = _target_acceptance_radius;

            if (_speed_at_target < 0.01f) {
                // If vehicle wants to stop at the target, then set acceptance radius to zero as well.
                acceptance_radius = 0.0f;
            }

            if ((target_threshold - acceptance_radius) >= SIGMA_NORM) {

                // Slow down depending on distance to target minus acceptance radius.
                float m = (speed_threshold - _speed_at_target) / (target_threshold - acceptance_radius);
                speed_sp_track = m * (closest_to_dest.length() - acceptance_radius) + _speed_at_target; // speed at transition

            } else {
                speed_sp_track = _speed_at_target;
            }

            // If we are close to target and the previous speed setpoint along track was smaller than
            // current speed setpoint along track, then take over the previous one.
            // This ensures smoothness since we anyway want to slow down.
            if ((speed_sp_prev_track < speed_sp_track)
                && (speed_sp_track * speed_sp_prev_track > 0.0f)
                && (speed_sp_prev_track > _speed_at_target)) {
                speed_sp_track = speed_sp_prev_track;
            }

        } else {

            // Vehicle is still far from destination. Accelerate or keep maximum target speed.
            float acc_track = (speed_sp_track - speed_sp_prev_track) / _deltatime;

            // If yaw offset is large, only accelerate with 0.5 m/s^2.
            float acc_max = (_yaw_sp_aligned) ? _param_mpc_acc_hor.get() : 0.5f;

            if (acc_track > acc_max) {
                // accelerate towards target
                speed_sp_track = acc_max * _deltatime + speed_sp_prev_track;
            }

            // ensure that desired speed does not exceed speed at threshold
            speed_sp_track = math::min(speed_threshold, speed_sp_track);
        }

        speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);

        _position_setpoint(0) = closest_pt(0);
        _position_setpoint(1) = closest_pt(1);
        Vector2f velocity_sp_xy = u_prev_to_dest * speed_sp_track;
        _velocity_setpoint(0) =  velocity_sp_xy(0);
        _velocity_setpoint(1) =  velocity_sp_xy(1);
    }
}

void FlightTaskAutoLine::_generateAltitudeSetpoints()
{
    // Total distance between previous and target set-point.
    const float dist = fabsf(_target(2) - _prev_wp(2));

    // If target has not been reached, then compute set-point depending on maximum velocity.
    if ((dist > SIGMA_NORM) && (fabsf(_position(2) - _target(2)) > 0.1f)) {

        // get various distances */
        const float dist_to_prev = fabsf(_position(2) - _prev_wp(2));
        const float dist_to_target = fabsf(_target(2) - _position(2));

        // check sign
        const bool flying_upward = _target(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,
                        _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
                        _param_mpc_land_speed.get(), _constraints.speed_down);

        // 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) ? _constraints.speed_up : vel_limit;

        // Target threshold defines the distance to target(2) at which
        // the vehicle starts to slow down to approach the target smoothly.
        float target_threshold = speed_sp * 1.5f;

        // If the total distance in z is NOT 2x distance of target_threshold, we
        // will need to adjust the final_velocity in z.
        const bool is_2_target_threshold = dist >= 2.0f * target_threshold;
        const float min_vel = 0.2f; // minimum velocity: this is needed since estimation is not perfect
        const float slope = (speed_sp - min_vel) / (target_threshold); // defines the the acceleration when slowing down */

        if (!is_2_target_threshold) {
            // Adjust final_velocity since we are already are close to target and therefore it is not necessary to accelerate
            // upwards with full speed.
            target_threshold = dist * 0.5f;
            // get the velocity at target_threshold
            speed_sp = slope * (target_threshold) + min_vel;
        }

        // we want to slow down
        if (dist_to_target < target_threshold) {

            speed_sp = slope * dist_to_target + min_vel;

        } else if (dist_to_prev < target_threshold) {
            // we want to accelerate

            const float acc = (speed_sp - fabsf(_velocity_setpoint(2))) / _deltatime;
            const float acc_max = (flying_upward) ? (_param_mpc_acc_up_max.get() * 0.5f) : (_param_mpc_acc_down_max.get() * 0.5f);

            if (acc > acc_max) {
                speed_sp = acc_max * _deltatime + fabsf(_velocity_setpoint(2));
            }
        }

        // make sure vel_sp_z is always positive
        if (speed_sp < 0.0f) {
            PX4_WARN("speed cannot be smaller than 0");
            speed_sp = 0.0f;
        }

        // get the sign of vel_sp_z
        _velocity_setpoint(2) = (flying_upward) ? -speed_sp : speed_sp;
        _position_setpoint(2) = NAN; // We don't care about position setpoint

    } else {

        // vehicle reached desired target altitude
        _velocity_setpoint(2) = 0.0f;
        _position_setpoint(2) = _target(2);
    }
}