FlightTaskAutoMapper2.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 FlightAutoMapper2.cpp
 */

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

using namespace matrix;

bool FlightTaskAutoMapper2::activate(vehicle_local_position_setpoint_s last_setpoint)
{
    bool ret = FlightTaskAuto::activate(last_setpoint);
    _reset();
    return ret;
}

bool FlightTaskAutoMapper2::update()
{
    // always reset constraints because they might change depending on the type
    _setDefaultConstraints();

    _updateAltitudeAboveGround();

    // The only time a thrust set-point is sent out is during
    // idle. Hence, reset thrust set-point to NAN in case the
    // vehicle exits idle.

    if (_type_previous == WaypointType::idle) {
        _thrust_setpoint = Vector3f(NAN, NAN, NAN);
    }

    // during mission and reposition, raise the landing gears but only
    // if altitude is high enough
    if (_highEnoughForLandingGear()) {
        _gear.landing_gear = landing_gear_s::GEAR_UP;
    }

    switch (_type) {
    case WaypointType::idle:
        _prepareIdleSetpoints();
        break;

    case WaypointType::land:
        _prepareLandSetpoints();
        break;

    case WaypointType::loiter:

    /* fallthrought */
    case WaypointType::position:
        _preparePositionSetpoints();
        break;

    case WaypointType::takeoff:
        _prepareTakeoffSetpoints();
        break;

    case WaypointType::velocity:
        _prepareVelocitySetpoints();
        break;

    default:
        _preparePositionSetpoints();
        break;
    }

    if (_param_com_obs_avoid.get()) {
        _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
        _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
                _yawspeed_setpoint);
    }


    _generateSetpoints();

    // update previous type
    _type_previous = _type;

    return true;
}

void FlightTaskAutoMapper2::_reset()
{
    // Set setpoints equal current state.
    _velocity_setpoint = _velocity;
    _position_setpoint = _position;
}

void FlightTaskAutoMapper2::_prepareIdleSetpoints()
{
    // Send zero thrust setpoint
    _position_setpoint = Vector3f(NAN, NAN, NAN); // Don't require any position/velocity setpoints
    _velocity_setpoint = Vector3f(NAN, NAN, NAN);
    _thrust_setpoint.zero();
}

void FlightTaskAutoMapper2::_prepareLandSetpoints()
{
    float land_speed = _getLandSpeed();

    // Keep xy-position and go down with landspeed
    _position_setpoint = Vector3f(_target(0), _target(1), NAN);
    _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed));

    // set constraints
    _constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
    _gear.landing_gear = landing_gear_s::GEAR_DOWN;
}

void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
{
    // Takeoff is completely defined by target position
    _position_setpoint = _target;
    const float speed_tko = (_alt_above_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up :
                _param_mpc_tko_speed.get();
    _velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed

    _gear.landing_gear = landing_gear_s::GEAR_DOWN;
}

void FlightTaskAutoMapper2::_prepareVelocitySetpoints()
{
    // XY Velocity waypoint
    // TODO : Rewiew that. What is the expected behavior?
    _position_setpoint = Vector3f(NAN, NAN, _position(2));
    Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
    _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
}

void FlightTaskAutoMapper2::_preparePositionSetpoints()
{
    // Simple waypoint navigation: go to xyz target, with standard limitations
    _position_setpoint = _target;
    _velocity_setpoint = Vector3f(NAN, NAN, NAN); // No special velocity limitations
}

void FlightTaskAutoMapper2::_updateAltitudeAboveGround()
{
    // Altitude above ground is by default just the negation of the current local position in D-direction.
    _alt_above_ground = -_position(2);

    if (PX4_ISFINITE(_dist_to_bottom)) {
        // We have a valid distance to ground measurement
        _alt_above_ground = _dist_to_bottom;

    } else if (_sub_home_position.get().valid_alt) {
        // if home position is set, then altitude above ground is relative to the home position
        _alt_above_ground = -_position(2) + _sub_home_position.get().z;
    }
}

void FlightTaskAutoMapper2::updateParams()
{
    FlightTaskAuto::updateParams();

    // make sure that alt1 is above alt2
    _param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
}

bool FlightTaskAutoMapper2::_highEnoughForLandingGear()
{
    // return true if altitude is above two meters
    return _alt_above_ground > 2.0f;
}

float FlightTaskAutoMapper2::_getLandSpeed()
{
    bool rc_assist_enabled = _param_mpc_land_rc_help.get();
    bool rc_is_valid = !_sub_vehicle_status.get().rc_signal_lost;

    float throttle = 0.5f;

    if (rc_is_valid && rc_assist_enabled) {
        throttle = _sub_manual_control_setpoint.get().z;
    }

    float speed = 0;

    if (_alt_above_ground > _param_mpc_land_alt1.get()) {
        speed = _constraints.speed_down;

    } else {
        float land_speed = _param_mpc_land_speed.get();
        float head_room = _constraints.speed_down - land_speed;

        speed = land_speed + 2 * (0.5f - throttle) * head_room;

        // Allow minimum assisted land speed to be half of parameter
        if (speed < land_speed * 0.5f) {
            speed = land_speed * 0.5f;
        }
    }

    return speed;
}