PositionControl.hpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2018 - 2019 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 PositionControl.hpp
 *
 * A cascaded position controller for position/velocity control only.
 */

#pragma once

#include <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_constraints.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>

struct PositionControlStates {
    matrix::Vector3f position;
    matrix::Vector3f velocity;
    matrix::Vector3f acceleration;
    float yaw;
};

/**
 *  Core Position-Control for MC.
 *  This class contains P-controller for position and
 *  PID-controller for velocity.
 *  Inputs:
 *      vehicle position/velocity/yaw
 *      desired set-point position/velocity/thrust/yaw/yaw-speed
 *      constraints that are stricter than global limits
 *  Output
 *      thrust vector and a yaw-setpoint
 *
 *  If there is a position and a velocity set-point present, then
 *  the velocity set-point is used as feed-forward. If feed-forward is
 *  active, then the velocity component of the P-controller output has
 *  priority over the feed-forward component.
 *
 *  A setpoint that is NAN is considered as not set.
 *  If there is a position/velocity- and thrust-setpoint present, then
 *  the thrust-setpoint is ommitted and recomputed from position-velocity-PID-loop.
 */
class PositionControl
{
public:

    PositionControl() = default;
    ~PositionControl() = default;

    /**
     * Set the position control gains
     * @param P 3D vector of proportional gains for x,y,z axis
     */
    void setPositionGains(const matrix::Vector3f &P) { _gain_pos_p = P; }

    /**
     * Set the velocity control gains
     * @param P 3D vector of proportional gains for x,y,z axis
     * @param I 3D vector of integral gains
     * @param D 3D vector of derivative gains
     */
    void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);

    /**
     * Set the maximum velocity to execute with feed forward and position control
     * @param vel_horizontal horizontal velocity limit
     * @param vel_up upwards velocity limit
     * @param vel_down downwards velocity limit
     */
    void setVelocityLimits(const float vel_horizontal, const float vel_up, float vel_down);

    /**
     * Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller
     * @param min minimum thrust e.g. 0.1 or 0
     * @param max maximum thrust e.g. 0.9 or 1
     */
    void setThrustLimits(const float min, const float max);

    /**
     * Set the maximum tilt angle in radians the output attitude is allowed to have
     * @param tilt angle from level orientation in radians
     */
    void setTiltLimit(const float tilt) { _lim_tilt = tilt; }

    /**
     * Set the maximum tilt angle in radians the output attitude is allowed to have
     * @param thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation
     */
    void setHoverThrust(const float thrust) { _hover_thrust = thrust; }

    /**
     * Update the current vehicle state.
     * @param PositionControlStates structure
     */
    void updateState(const PositionControlStates &states);

    /**
     * Update the desired setpoints.
     * @param setpoint a vehicle_local_position_setpoint_s structure
     * @return true if setpoint has updated correctly
     */
    bool updateSetpoint(const vehicle_local_position_setpoint_s &setpoint);

    /**
     * Set constraints that are stricter than the global limits.
     * @param constraints a PositionControl structure with supported constraints
     */
    void updateConstraints(const vehicle_constraints_s &constraints);

    /**
     * Apply P-position and PID-velocity controller that updates the member
     * thrust, yaw- and yawspeed-setpoints.
     * @see _thr_sp
     * @see _yaw_sp
     * @see _yawspeed_sp
     * @param dt the delta-time
     */
    void generateThrustYawSetpoint(const float dt);

    /**
     *  Set the integral term in xy to 0.
     *  @see _thr_int
     */
    void resetIntegralXY() { _thr_int(0) = _thr_int(1) = 0.f; }

    /**
     *  Set the integral term in z to 0.
     *  @see _thr_int
     */
    void resetIntegralZ() { _thr_int(2) = 0.f; }

    /**
     *  Get the
     *  @see _vel_sp
     *  @return The velocity set-point that was executed in the control-loop. Nan if velocity control-loop was skipped.
     */
    const matrix::Vector3f getVelSp() const
    {
        matrix::Vector3f vel_sp{};

        for (int i = 0; i <= 2; i++) {
            if (_ctrl_vel[i]) {
                vel_sp(i) = _vel_sp(i);

            } else {
                vel_sp(i) = NAN;
            }
        }

        return vel_sp;
    }

    /**
     * Get the controllers output local position setpoint
     * These setpoints are the ones which were executed on including PID output and feed-forward.
     * The acceleration or thrust setpoints can be used for attitude control.
     * @param local_position_setpoint reference to struct to fill up
     */
    void getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const;

    /**
     * Get the controllers output attitude setpoint
     * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control.
     * It needs to be executed by the attitude controller to achieve velocity and position tracking.
     * @param attitude_setpoint reference to struct to fill up
     */
    void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;

private:
    /**
     * Maps setpoints to internal-setpoints.
     * @return true if mapping succeeded.
     */
    bool _interfaceMapping();

    void _positionController(); /** applies the P-position-controller */
    void _velocityController(const float &dt); /** applies the PID-velocity-controller */
    void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */

    // Gains
    matrix::Vector3f _gain_pos_p; ///< Position control proportional gain
    matrix::Vector3f _gain_vel_p; ///< Velocity control proportional gain
    matrix::Vector3f _gain_vel_i; ///< Velocity control integral gain
    matrix::Vector3f _gain_vel_d; ///< Velocity control derivative gain

    // Limits
    float _lim_vel_horizontal{}; ///< Horizontal velocity limit with feed forward and position control
    float _lim_vel_up{}; ///< Upwards velocity limit with feed forward and position control
    float _lim_vel_down{}; ///< Downwards velocity limit with feed forward and position control
    float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9
    float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1
    float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have

    float _hover_thrust{}; ///< Thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation

    // States
    matrix::Vector3f _pos; /**< current position */
    matrix::Vector3f _vel; /**< current velocity */
    matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */
    matrix::Vector3f _thr_int; /**< integral term of the velocity controller */
    float _yaw{}; /**< current heading */

    vehicle_constraints_s _constraints{}; /**< variable constraints */

    // Setpoints
    matrix::Vector3f _pos_sp; /**< desired position */
    matrix::Vector3f _vel_sp; /**< desired velocity */
    matrix::Vector3f _acc_sp; /**< desired acceleration */
    matrix::Vector3f _thr_sp; /**< desired thrust */
    float _yaw_sp{}; /**< desired heading */
    float _yawspeed_sp{}; /** desired yaw-speed */

    bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */
    bool _ctrl_pos[3] = {true, true, true}; /**< True if the control-loop for position was used */
    bool _ctrl_vel[3] = {true, true, true}; /**< True if the control-loop for velocity was used */
};