fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 04:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl/mc_pos_control: update comments

Browse Source
This commit is contained in:
Dennis Mannhart
2018-03-27 17:02:25 +02:00
committed by Lorenz Meier
parent 673945f28b
commit cf59c2a2ac
2 changed files with 220 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
+73 -78
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* 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
@@ -33,13 +33,6 @@
/**
* @file PositionControl.cpp
*
* This file implements a P-position-control cascaded with a
* PID-velocity-controller.
*
* Inputs: vehicle states (pos, vel, q)
* desired setpoints (pos, vel, thrust, yaw)
* Outputs: thrust and yaw setpoint
*/
#include "PositionControl.hpp"
@@ -69,7 +62,7 @@ PositionControl::PositionControl()
_ThrMinStab_h = param_find("MPC_MANTHR_MIN");
_parameter_sub = orb_subscribe(ORB_ID(parameter_update));
/* Set parameter the very first time. */
// set parameter the very first time
_setParams();
};
@@ -91,9 +84,8 @@ void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
_yawspeed_sp = setpoint.yawspeed;
_interfaceMapping();
/* If full manual is required (thrust already generated), don't run position/velocity
* controller and just return thrust.
*/
// If full manual is required (thrust already generated), don't run position/velocity
// controller and just return thrust.
_skipController = false;
_ThrustLimit.min = _ThrMinPosition;
@@ -109,9 +101,8 @@ void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
void PositionControl::generateThrustYawSetpoint(const float &dt)
{
/* Only run position/velocity controller
* if thrust needs to be generated
*/
// Only run position/velocity controller
// if thrust needs to be generated.
if (!_skipController) {
_positionController();
_velocityController(dt);
@@ -120,44 +111,40 @@ void PositionControl::generateThrustYawSetpoint(const float &dt)
void PositionControl::_interfaceMapping()
{
/* Respects FlightTask interface, where
* NAN-setpoints are of no interest and
* do not require control.
*/
/* Loop through x,y and z components of all setpoints. */
// Respects FlightTask interface, where NAN-set-points are of no interest
// and do not require control. A valide position and velocity setpoint will
// be mapped to a desired position setpoint with a feed-forward term.
for (int i = 0; i <= 2; i++) {
if (PX4_ISFINITE(_pos_sp(i))) {
/* Position control is required */
// Position control is required
if (!PX4_ISFINITE(_vel_sp(i))) {
/* Velocity is not used as feedforward term. */
// Velocity is not used as feedforward term.
_vel_sp(i) = 0.0f;
}
/* thrust setpoint is not supported
* in position control
*/
// thrust setpoint is not supported in position control
_thr_sp(i) = 0.0f;
} else if (PX4_ISFINITE(_vel_sp(i))) {
/* Velocity controller is active without
* position control.
*/
// Velocity controller is active without position control.
// Set the desired position set-point equal to current position
// such that error is zero.
_pos_sp(i) = _pos(i);
// thrust setpoint is not supported in position control
_thr_sp(i) = 0.0f;
} else if (PX4_ISFINITE(_thr_sp(i))) {
/* Thrust setpoint was generated from
* stick directly.
*/
// Thrust setpoint was generated from sticks directly.
// Set all other set-points equal MC states.
_pos_sp(i) = _pos(i);
_vel_sp(i) = _vel(i);
// Reset the Integral term.
_thr_int(i) = 0.0f;
// Don't require velocity derivative.
_vel_dot(i) = 0.0f;
} else {
@@ -166,67 +153,71 @@ void PositionControl::_interfaceMapping()
}
if (!PX4_ISFINITE(_yawspeed_sp)) {
// Set the yawspeed to 0 since not used.
_yawspeed_sp = 0.0f;
}
if (!PX4_ISFINITE(_yaw_sp)) {
// Set the yaw-sp eaual the current yaw.
// That is the best we can do and it also
// agrees with FlightTask-interface definition.
_yaw_sp = _yaw;
}
}
void PositionControl::_positionController()
{
/* Generate desired velocity setpoint */
/* P-controller */
// P-position controller
Vector3f vel_sp_position = (_pos_sp - _pos).emult(Pp);
_vel_sp = vel_sp_position + _vel_sp;
// Constrain horizontal velocity by prioritizing the velocity component along the
// the desired position setpoint over the feed-forward term.
Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(&vel_sp_position(0)), Vector2f(&(_vel_sp - vel_sp_position)(0)),
_VelMaxXY);
_vel_sp(0) = vel_sp_xy(0);
_vel_sp(1) = vel_sp_xy(1);
// Constrain velocity in z-directio.
_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ.up, _VelMaxZ.down);
}
void PositionControl::_velocityController(const float &dt)
{
/* Generate desired thrust setpoint */
/* PID
* u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
* Umin <= u_des <= Umax
*
* Anti-Windup:
* u_des = _thr_sp; r = _vel_sp; y = _vel
* u_des >= Umax and r - y >= 0 => Saturation = true
* u_des >= Umax and r - y <= 0 => Saturation = false
* u_des <= Umin and r - y <= 0 => Saturation = true
* u_des <= Umin and r - y >= 0 => Saturation = false
*
* Notes:
* - PID implementation is in NED-frame
* - control output in D-direction has priority over NE-direction
* - the equilibrium point for the PID is at hover-thrust
* - the maximum tilt cannot exceed 90 degrees. This means that it is
* not possible to have a desired thrust direction pointing in the positive
* D-direction (= downward)
* - the desired thrust in D-direction is limited by the thrust limits
* - the desired thrust in NE-direction is limited by the thrust excess after
* consideration of the desired thrust in D-direction. In addition, the thrust in
* NE-direction is also limited by the maximum tilt.
*/
// Generate desired thrust setpoint.
// PID
// u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
// Umin <= u_des <= Umax
//
// Anti-Windup:
// u_des = _thr_sp; r = _vel_sp; y = _vel
// u_des >= Umax and r - y >= 0 => Saturation = true
// u_des >= Umax and r - y <= 0 => Saturation = false
// u_des <= Umin and r - y <= 0 => Saturation = true
// u_des <= Umin and r - y >= 0 => Saturation = false
//
// Notes:
// - PID implementation is in NED-frame
// - control output in D-direction has priority over NE-direction
// - the equilibrium point for the PID is at hover-thrust
// - the maximum tilt cannot exceed 90 degrees. This means that it is
// not possible to have a desired thrust direction pointing in the positive
// D-direction (= downward)
// - the desired thrust in D-direction is limited by the thrust limits
// - the desired thrust in NE-direction is limited by the thrust excess after
// consideration of the desired thrust in D-direction. In addition, the thrust in
// NE-direction is also limited by the maximum tilt.
Vector3f vel_err = _vel_sp - _vel;
/* Consider thrust in D-direction */
// Consider thrust in D-direction.
float thrust_desired_D = Pv(2) * vel_err(2) + Dv(2) * _vel_dot(2) + _thr_int(2) - _ThrHover;
/* The Thrust limits are negated and swapped due to NED-frame */
// The Thrust limits are negated and swapped due to NED-frame.
float uMax = -_ThrustLimit.min;
float uMin = -_ThrustLimit.max;
/* Apply Anti-Windup in D-direction */
// Apply Anti-Windup in D-direction.
bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) ||
(thrust_desired_D <= uMin && vel_err(2) <= 0.0f);
@@ -235,35 +226,31 @@ void PositionControl::_velocityController(const float &dt)
}
/* Saturate thrust setpoint in D-direction */
// Saturate thrust setpoint in D-direction.
_thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax);
if (fabsf(_thr_sp(0)) + fabsf(_thr_sp(1)) > FLT_EPSILON) {
/* Thrust setpoints in NE-direction is already provided. Only
* scaling is required.
*/
// Thrust set-point in NE-direction is already provided. Only
// scaling by the maximum tilt is required.
float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_tilt_max);
_thr_sp(0) *= thr_xy_max;
_thr_sp(1) *= thr_xy_max;
} else {
/* PID for NE-direction */
// PID-velocity controller for NE-direction.
Vector2f thrust_desired_NE;
thrust_desired_NE(0) = Pv(0) * vel_err(0) + Dv(0) * _vel_dot(0) + _thr_int(0);
thrust_desired_NE(1) = Pv(1) * vel_err(1) + Dv(1) * _vel_dot(1) + _thr_int(1);
/* Get maximum allowed thrust in NE based on tilt and excess thrust */
// Get maximum allowed thrust in NE based on tilt and excess thrust.
float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_tilt_max);
float thrust_max_NE = sqrtf(_ThrustLimit.max * _ThrustLimit.max - _thr_sp(2) * _thr_sp(2));
thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
/* Get the direction of (r-y) in NE-direction */
// Get the direction of (r-y) in NE-direction.
float direction_NE = Vector2f(&vel_err(0)) * Vector2f(&_vel_sp(0));
/* Apply Anti-Windup in NE-direction */
// Apply Anti-Windup in NE-direction.
bool stop_integral_NE = (thrust_desired_NE * thrust_desired_NE >= thrust_max_NE * thrust_max_NE &&
direction_NE >= 0.0f);
@@ -272,7 +259,7 @@ void PositionControl::_velocityController(const float &dt)
_thr_int(1) += vel_err(1) * Iv(1) * dt;
}
/* Saturate thrust in NE-direction */
// Saturate thrust in NE-direction.
_thr_sp(0) = thrust_desired_NE(0);
_thr_sp(1) = thrust_desired_NE(1);
@@ -289,10 +276,18 @@ void PositionControl::updateConstraints(const Controller::Constraints &constrain
{
// update all parameters since they might have changed
_updateParams();
// maximum tilt cannot exceed 90 degrees
_tilt_max = math::min(constraints.tilt_max, M_PI_2_F);
_tilt_max = M_PI_2_F;
if (PX4_ISFINITE(constraints.tilt_max)) {
_tilt_max = math::min(constraints.tilt_max, _tilt_max);
}
// maximum velocity upwards cannot exceed global limit
_VelMaxZ.up = math::min(constraints.vel_max_z_up, _VelMaxZ.up);
if (PX4_ISFINITE(constraints.vel_max_z_up)) {
_VelMaxZ.up = math::min(constraints.vel_max_z_up, _VelMaxZ.up);
}
}
void PositionControl::_updateParams()
src/modules/mc_pos_control/PositionControl.hpp
+147 -60
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* 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
@@ -34,9 +34,7 @@
/**
* @file PositionControl.hpp
*
* @inputs: position-, velocity-, acceleration-, thrust-setpoints
* @outputs: thrust vector
*
* A cascaded position controller for position/velocity control only.
*/
#include <matrix/matrix/math.hpp>
@@ -47,100 +45,189 @@
#pragma once
/* Constraints based on mode:
* Eventually this structure should be part of local position message
*/
namespace Controller
{
/** Constraints that depends on mode and are lower
* than the global limits.
* tilt_max: Cannot exceed PI/2
* vel_max_z_up: Cannot exceed maximum global velocity upwards
* @see _tilt_max
* @see _VelMaxZ
*/
struct Constraints {
float tilt_max;
float vel_max_z_up;
float tilt_max; /**< maximum tilt always below Pi/2 */
float vel_max_z_up; /**< maximum speed upwards always smaller than MPC_VEL_Z_MAX_UP */
};
}
/**
* 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.
*/
class PositionControl
{
public:
PositionControl();
PositionControl();
~PositionControl() {};
/**
* Update the current vehicle state.
* @param state a vehicle_local_position_s structure
* @param vel_dot the derivative of the vehicle velocity
*/
void updateState(const vehicle_local_position_s &state, const matrix::Vector3f &vel_dot);
/**
* Update the desired setpoints.
* @param setpoint a vehicle_local_position_setpoint_s structure
*/
void 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 Controller::Constraints &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 _thr_sp
* @return The thrust set-point member.
*/
matrix::Vector3f getThrustSetpoint() {return _thr_sp;}
/**
* Get the
* @see _yaw_sp
* @return The yaw set-point member.
*/
float getYawSetpoint() { return _yaw_sp;}
/**
* Get the
* @see _yawspeed_sp
* @return The yawspeed set-point member.
*/
float getYawspeedSetpoint() {return _yawspeed_sp;}
/**
* Get the
* @see _vel_sp
* @return The velocity set-point member.
*/
matrix::Vector3f getVelSp() {return _vel_sp;}
/**
* Get the
* @see _pos_sp
* @return The position set-point member.
*/
matrix::Vector3f getPosSp() {return _pos_sp;}
private:
/* States */
matrix::Vector3f _pos{};
matrix::Vector3f _vel{};
matrix::Vector3f _vel_dot{};
matrix::Vector3f _acc{};
float _yaw{0.0f};
matrix::Vector3f _pos{}; /**< MC position */
matrix::Vector3f _vel{}; /**< MC velocity */
matrix::Vector3f _vel_dot{}; /**< MC velocity derivative */
matrix::Vector3f _acc{}; /**< MC acceleration */
float _yaw{0.0f}; /**< MC yaw */
/* Setpoints */
matrix::Vector3f _pos_sp{};
matrix::Vector3f _vel_sp{};
matrix::Vector3f _acc_sp{};
matrix::Vector3f _thr_sp{};
float _yaw_sp{};
float _yawspeed_sp{};
matrix::Vector3f _pos_sp{}; /**< desired position */
matrix::Vector3f _vel_sp{}; /**< desired velocity */
matrix::Vector3f _acc_sp{}; /**< desired acceleration: not supported yet */
matrix::Vector3f _thr_sp{}; /**< desired thrust */
float _yaw_sp{}; /**< desired yaw */
float _yawspeed_sp{}; /** desired yaw-speed */
/* Other variables */
matrix::Vector3f _thr_int{};
Controller::Constraints _constraints{};
matrix::Vector3f _thr_int{}; /**< thrust integral term */
Controller::Constraints _constraints{}; /**< variable constraints */
/* Parameter handles */
int _parameter_sub{-1};
param_t _Pz_h{PARAM_INVALID};
param_t _Pvz_h{PARAM_INVALID};
param_t _Ivz_h{PARAM_INVALID};
param_t _Dvz_h{PARAM_INVALID};
param_t _Pxy_h{PARAM_INVALID};
param_t _Pvxy_h{PARAM_INVALID};
param_t _Ivxy_h{PARAM_INVALID};
param_t _Dvxy_h{PARAM_INVALID};
param_t _VelMaxXY_h{PARAM_INVALID};
param_t _VelMaxZdown_h{PARAM_INVALID};
param_t _VelMaxZup_h{PARAM_INVALID};
param_t _ThrHover_h{PARAM_INVALID};
param_t _ThrMax_h{PARAM_INVALID};
param_t _ThrMinPosition_h{PARAM_INVALID};
param_t _ThrMinStab_h{PARAM_INVALID};
/* Parameters */
/**
* Position Gains.
* Pp: P-controller gain for position-controller
* Pv: P-controller gain for velocity-controller
* Iv: I-controller gain for velocity-controller
* Dv: D-controller gain for velocity-controller
*/
matrix::Vector3f Pp, Pv, Iv, Dv = matrix::Vector3f{0.0f, 0.0f, 0.0f};
float _VelMaxXY{};
float _VelMaxXY{}; /**< maximum global limit for velocity in the horizontal direction */
struct DirectionD {
float up;
float down;
};
DirectionD _VelMaxZ;
DirectionD _VelMaxZ; /**< struct for velocity limits in the z-direction */
struct Limits {
float max;
float min;
};
Limits _ThrustLimit;
Limits _ThrustLimit; /**< struct for thrust-limits */
float _ThrHover{0.5f};
float _ThrMinPosition{0.0f}; // minimum throttle for any position controlled mode
float _ThrMinStab{0.0f}; // minimum throttle for stabilized
float _tilt_max{1.5f}; /**< maximum tilt */
bool _skipController{false};
float _ThrHover{0.5f}; /** equilibrium point for the velocity controller */
float _ThrMinPosition{0.0f}; /**< minimum throttle for any position controlled mode */
float _ThrMinStab{0.0f}; /**< minimum throttle for stabilized mode */
float _tilt_max{1.5f}; /**< maximum tilt for any velocity controlled mode. */
bool _skipController{false}; /**< skips position/velocity controller. true for stabilized mode */
/* Helper methods */
void _interfaceMapping();
void _positionController();
void _velocityController(const float &dt);
void _updateParams();
void _setParams();
void _interfaceMapping(); /** maps setpoints to internal member setpoints */
void _positionController(); /** applies the P-position-controller */
void _velocityController(const float &dt); /** applies the PID-velocity-controller */
void _updateParams(); /** updates parameters */
void _setParams(); /** sets parameters to internal member */
// Parameter handles
int _parameter_sub { -1 };
param_t _Pz_h { PARAM_INVALID };
param_t _Pvz_h { PARAM_INVALID };
param_t _Ivz_h { PARAM_INVALID };
param_t _Dvz_h { PARAM_INVALID };
param_t _Pxy_h { PARAM_INVALID };
param_t _Pvxy_h { PARAM_INVALID };
param_t _Ivxy_h { PARAM_INVALID };
param_t _Dvxy_h { PARAM_INVALID };
param_t _VelMaxXY_h { PARAM_INVALID };
param_t _VelMaxZdown_h { PARAM_INVALID };
param_t _VelMaxZup_h { PARAM_INVALID };
param_t _ThrHover_h { PARAM_INVALID };
param_t _ThrMax_h { PARAM_INVALID };
param_t _ThrMinPosition_h { PARAM_INVALID };
param_t _ThrMinStab_h { PARAM_INVALID };
};