mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-18 18:39:06 +08:00
TranslationControl: update comments
This commit is contained in:
Dennis Mannhart
Beat Küng
parent
720271332d
commit
6a4e3bb94e
@ -73,7 +73,7 @@ TranslationControl::TranslationControl()
|
||||
_YawRateMax_h = param_find("MPC_MAN_Y_MAX");
|
||||
_Pyaw_h = param_find("MC_YAW_P");
|
||||
|
||||
/* Set parameter the very first time */
|
||||
/* Set parameter the very first time. */
|
||||
_setParams();
|
||||
};
|
||||
|
||||
@ -100,37 +100,42 @@ void TranslationControl::updateSetpoint(struct vehicle_local_position_setpoint_s
|
||||
void TranslationControl::generateThrustYawSetpoint(const float &dt)
|
||||
{
|
||||
_updateParams();
|
||||
|
||||
_positionController();
|
||||
|
||||
_velocityController(dt);
|
||||
|
||||
_yawController(dt);
|
||||
|
||||
}
|
||||
|
||||
void TranslationControl::_interfaceMapping()
|
||||
{
|
||||
/* loop through x,y and z component */
|
||||
/* 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. */
|
||||
for (int i = 0; i <= 2; i++) {
|
||||
|
||||
if (PX4_ISFINITE(_pos_sp(i))) {
|
||||
|
||||
/* Check if velocity should be used as feedforward */
|
||||
/* Position control is required */
|
||||
|
||||
if (!PX4_ISFINITE(_vel_sp(i))) {
|
||||
/* Velocity is not used as feedforward term. */
|
||||
_vel_sp(i) = 0.0f;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
/* Position controller is not active
|
||||
* -> Set setpoint just to current position since */
|
||||
/* Velocity controller is active without
|
||||
* position control.
|
||||
*/
|
||||
_pos_sp(i) = _pos(i);
|
||||
|
||||
/* check if velocity setpoint should be used */
|
||||
if (!PX4_ISFINITE(_vel_sp(i))) {
|
||||
/* No position/velocity controller active.
|
||||
* Attitude will be generated from sticks directly
|
||||
* TODO: Adjust to the new FlightTask interface
|
||||
* that also sends thrust setpoints.
|
||||
*/
|
||||
_vel_sp(i) = _vel(i);
|
||||
}
|
||||
@ -138,25 +143,34 @@ void TranslationControl::_interfaceMapping()
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_yawspeed_sp)) {
|
||||
|
||||
/* Targe yaw is yaw setpoint. No need for yawspped */
|
||||
_yawspeed_sp = 0.0f;
|
||||
|
||||
if (!PX4_ISFINITE(_yaw_sp)) {
|
||||
|
||||
/* There is no finite setpoint. The best
|
||||
* we can do is to just re-use old setpoint */
|
||||
_yaw_sp = _yaw_sp_int;
|
||||
}
|
||||
|
||||
} else if (!PX4_ISFINITE(_yaw_sp)) {
|
||||
/* Nothing is finite: Best we can do is to just
|
||||
* reuse old setpoint.
|
||||
*/
|
||||
_yaw_sp = _yaw_sp_int;
|
||||
}
|
||||
}
|
||||
|
||||
/* generate desired velocity */
|
||||
void TranslationControl::_positionController()
|
||||
{
|
||||
/* Generate desired velocity setpoint */
|
||||
|
||||
/* P-controller */
|
||||
_vel_sp = (_pos_sp - _pos).emult(Pp) + _vel_sp;
|
||||
|
||||
/* make sure velocity setpoint is constrained in all directions (xyz) */
|
||||
float vel_norm_xy = sqrtf(
|
||||
_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
|
||||
/* Make sure velocity setpoint is constrained in all directions (xyz). */
|
||||
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
|
||||
|
||||
if (vel_norm_xy > _VelMaxXY) {
|
||||
_vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_norm_xy;
|
||||
@ -166,9 +180,9 @@ void TranslationControl::_positionController()
|
||||
_vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ[0], _VelMaxZ[1]);
|
||||
}
|
||||
|
||||
/* generates desired thrust vector */
|
||||
void TranslationControl::_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
|
||||
@ -184,13 +198,15 @@ void TranslationControl::_velocityController(const float &dt)
|
||||
|
||||
Data vel_err = _vel_sp - _vel;
|
||||
|
||||
/* TODO: add offboard acceleration mode */
|
||||
/* TODO: add offboard acceleration mode
|
||||
* PID-controller */
|
||||
Data offset(0.0f, 0.0f, _ThrHover);
|
||||
_thr_sp = Pv.emult(vel_err) + Dv.emult(_vel_dot) + _thr_int - offset;
|
||||
|
||||
/* Limit tilt with priority on z
|
||||
* For manual controlled mode excluding pure manual and rate control, maximum tilt is 90;
|
||||
* It is to note that pure manual and rate control will never enter _velocityController method*/
|
||||
* It is to note that pure manual and rate control will never enter _velocityController method.
|
||||
* TODO: This needs to be revisited. */
|
||||
float tilt_max = PX4_ISFINITE(_constraints.tilt_max) ? _constraints.tilt_max : M_PI_2_F;
|
||||
tilt_max = math::min(tilt_max, M_PI_2_F);
|
||||
_thr_sp = ControlMath::constrainTilt(_thr_sp, tilt_max);
|
||||
@ -222,10 +238,10 @@ void TranslationControl::_velocityController(const float &dt)
|
||||
bool stop_I[2] = {false, false}; // stop integration for xy and z
|
||||
ControlMath::constrainPIDu(_thr_sp, stop_I, _ThrLimit, direction);
|
||||
|
||||
/* throttle is just thrust length */
|
||||
/* Throttle is just thrust length. */
|
||||
_throttle = _thr_sp.length();
|
||||
|
||||
/* update integrals */
|
||||
/* Update integrals */
|
||||
if (!stop_I[0]) {
|
||||
_thr_int(0) += vel_err(0) * Iv(0) * dt;
|
||||
_thr_int(1) += vel_err(1) * Iv(1) * dt;
|
||||
@ -251,7 +267,7 @@ void TranslationControl::_yawController(const float &dt)
|
||||
_yaw_sp = yaw_target;
|
||||
}
|
||||
|
||||
/* update yaw setpoint integral */
|
||||
/* Update yaw setpoint integral */
|
||||
_yaw_sp_int = _yaw_sp;
|
||||
|
||||
}
|
||||
|
||||
@ -34,7 +34,7 @@
|
||||
/**
|
||||
* @file TranslationControl.hpp
|
||||
*
|
||||
* @inputs: position-, velocity-, acceleration-, thrust- setpoints
|
||||
* @inputs: position-, velocity-, acceleration-, thrust-setpoints
|
||||
* @outputs: thrust vector
|
||||
*
|
||||
*/
|
||||
@ -47,7 +47,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
/* Variable constraints based on mode:
|
||||
/* Constraints based on mode:
|
||||
* Eventually this structure should be part of local position message
|
||||
*/
|
||||
namespace Controller
|
||||
@ -79,7 +79,7 @@ public:
|
||||
|
||||
private:
|
||||
|
||||
/* states */
|
||||
/* States */
|
||||
matrix::Vector3f _pos{};
|
||||
matrix::Vector3f _vel{};
|
||||
matrix::Vector3f _vel_dot{};
|
||||
@ -88,7 +88,7 @@ private:
|
||||
float _yaw{0.0f};
|
||||
matrix::Matrix<float, 3, 3> _R{};
|
||||
|
||||
/* setpoints */
|
||||
/* Setpoints */
|
||||
matrix::Vector3f _pos_sp{};
|
||||
matrix::Vector3f _vel_sp{};
|
||||
matrix::Vector3f _acc_sp{};
|
||||
@ -97,12 +97,12 @@ private:
|
||||
float _yawspeed_sp{};
|
||||
float _throttle{};
|
||||
|
||||
/* other variables */
|
||||
/* Other variables */
|
||||
matrix::Vector3f _thr_int{};
|
||||
float _yaw_sp_int{};
|
||||
Controller::Constraints _constraints{};
|
||||
|
||||
/* params handles */
|
||||
/* Parameter handles */
|
||||
int _parameter_sub{-1};
|
||||
param_t _Pz_h{PARAM_INVALID};
|
||||
param_t _Pvz_h{PARAM_INVALID};
|
||||
@ -121,7 +121,7 @@ private:
|
||||
param_t _YawRateMax_h{PARAM_INVALID};
|
||||
param_t _Pyaw_h{PARAM_INVALID}; //only temporary: this will be moved into attitude controller
|
||||
|
||||
/* params */
|
||||
/* Parameters */
|
||||
matrix::Vector3f Pp, Pv, Iv, Dv = matrix::Vector3f{0.0f, 0.0f, 0.0f};
|
||||
float _VelMaxXY{};
|
||||
float _VelMaxZ[2]; //index 0: index up; 1: down
|
||||
@ -130,7 +130,7 @@ private:
|
||||
float _Pyaw{};
|
||||
float _YawRateMax{};
|
||||
|
||||
/* helper methods */
|
||||
/* Helper methods */
|
||||
void _interfaceMapping();
|
||||
void _positionController();
|
||||
void _velocityController(const float &dt);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user