mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-02 10:50:35 +08:00
Matthias Grob
25a445d5f8
to make the class easier to understand, more modular and easily unit testable
249 lines
9.1 KiB
C++
249 lines
9.1 KiB
C++
/****************************************************************************
|
|
*
|
|
* 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.0f; }
|
|
|
|
/**
|
|
* Set the integral term in z to 0.
|
|
* @see _thr_int
|
|
*/
|
|
void resetIntegralZ() { _thr_int(2) = 0.0f; }
|
|
|
|
/**
|
|
* 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{0}; ///< Horizontal velocity limit with feed forward and position control
|
|
float _lim_vel_up{0}; ///< Upwards velocity limit with feed forward and position control
|
|
float _lim_vel_down{0}; ///< Downwards velocity limit with feed forward and position control
|
|
float _lim_thr_min{0}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9
|
|
float _lim_thr_max{0}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1
|
|
float _lim_tilt{0}; ///< Maximum tilt from level the output attitude is allowed to have
|
|
|
|
float _hover_thrust{0}; ///< Thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation
|
|
|
|
// States
|
|
matrix::Vector3f _pos; /**< position */
|
|
matrix::Vector3f _vel; /**< velocity */
|
|
matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */
|
|
matrix::Vector3f _thr_int; /**< integral term of the velocity controller */
|
|
float _yaw = 0.0f; /**< yaw */
|
|
|
|
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 yaw */
|
|
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 */
|
|
};
|