PositionControl.hpp

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.
 *
 ****************************************************************************/

#include <matrix/matrix/math.hpp>

#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_constraints.h>
#include <px4_module_params.h>
#pragma once

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

class PositionControl : public ModuleParams
{
public:

    PositionControl(ModuleParams *parent);
    ~PositionControl() = default;

    void overwriteParams();

    void updateState(const PositionControlStates &states);

    bool updateSetpoint(const vehicle_local_position_setpoint_s &setpoint);

    void updateConstraints(const vehicle_constraints_s &constraints);

    void generateThrustYawSetpoint(const float dt);

    void resetIntegralXY() { _thr_int(0) = _thr_int(1) = 0.0f; }

    void resetIntegralZ() { _thr_int(2) = 0.0f; }

    const matrix::Vector3f &getThrustSetpoint() { return _thr_sp; }

    const float &getYawSetpoint() { return _yaw_sp; }

    const float &getYawspeedSetpoint() { return _yawspeed_sp; }

    const matrix::Vector3f getVelSp()
    {
        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;
    }

    const matrix::Vector3f getPosSp()
    {
        matrix::Vector3f pos_sp{};

        for (int i = 0; i <= 2; i++) {
            if (_ctrl_pos[i]) {
                pos_sp(i) = _pos_sp(i);

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

        return pos_sp;
    }

protected:

    void updateParams() override;

private:
    bool _interfaceMapping();

    void _positionController(); 
    void _velocityController(const float &dt); 
    void _setCtrlFlag(bool value); 
    matrix::Vector3f _pos{}; 
    matrix::Vector3f _vel{}; 
    matrix::Vector3f _vel_dot{}; 
    matrix::Vector3f _acc{}; 
    float _yaw{0.0f}; 
    matrix::Vector3f _pos_sp{}; 
    matrix::Vector3f _vel_sp{}; 
    matrix::Vector3f _acc_sp{}; 
    matrix::Vector3f _thr_sp{}; 
    float _yaw_sp{}; 
    float _yawspeed_sp{}; 
    matrix::Vector3f _thr_int{}; 
    vehicle_constraints_s _constraints{}; 
    bool _skip_controller{false}; 
    bool _ctrl_pos[3] = {true, true, true}; 
    bool _ctrl_vel[3] = {true, true, true}; 
    DEFINE_PARAMETERS(
        (ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max,
        (ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
        (ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
        (ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min,
        (ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
        (ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
        (ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
        (ParamFloat<px4::params::MPC_TILTMAX_AIR>)
        _param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in radians
        (ParamFloat<px4::params::MPC_MAN_TILT_MAX>)
        _param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in radians
        (ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
        (ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p,
        (ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,
        (ParamFloat<px4::params::MPC_Z_VEL_D>) _param_mpc_z_vel_d,
        (ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
        (ParamFloat<px4::params::MPC_XY_VEL_P>) _param_mpc_xy_vel_p,
        (ParamFloat<px4::params::MPC_XY_VEL_I>) _param_mpc_xy_vel_i,
        (ParamFloat<px4::params::MPC_XY_VEL_D>) _param_mpc_xy_vel_d
    )
};