From f0f84d300b189990be13729d0fd21a5d3e4284f3 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Tue, 19 Dec 2017 08:48:35 +0100 Subject: [PATCH] TranslationControl: move mc_pos_control related logic into TranslationControl --- .../mc_pos_control/TranslationControl.cpp | 282 +++++++++++++++++- .../mc_pos_control/TranslationControl.hpp | 66 +++- 2 files changed, 344 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_pos_control/TranslationControl.cpp b/src/modules/mc_pos_control/TranslationControl.cpp index b5405529e1..b3b8f778f7 100644 --- a/src/modules/mc_pos_control/TranslationControl.cpp +++ b/src/modules/mc_pos_control/TranslationControl.cpp @@ -39,6 +39,9 @@ #include "TranslationControl.hpp" #include #include +#include "uORB/topics/parameter_update.h" +#include //TODO: only used for wrap_pi -> move this to mathlib since +// it makes more sense using namespace matrix; @@ -46,14 +49,35 @@ using Data = matrix::Vector3f; TranslationControl::TranslationControl() { + _Pz_h = param_find("MPC_Z_P"); + _Pvz_h = param_find("MPC_Z_VEL_P"); + _Ivz_h = param_find("MPC_Z_VEL_I"); + _Dvz_h = param_find("MPC_Z_VEL_D"); + _Pxy_h = param_find("MPC_XY_P"); + _Pvxy_h = param_find("MPC_XY_VEL_P"); + _Ivxy_h = param_find("MPC_XY_VEL_I"); + _Dvxy_h = param_find("MPC_XY_VEL_D"); + _VelMaxXY = param_find("MPC_XY_VEL_MAX"); + _VelMaxZdown_h = param_find("MPC_Z_VEL_MAX_DN"); + _VelMaxZup_h = param_find("MPC_Z_VEL_MAX_UP"); + _ThrHover_h = param_find("MPC_THR_HOVER"); + _ThrMax_h = param_find("MPC_THR_MAX"); + _ThrMin_h = param_find("MPC_THR_MIN"); + _YawRateMax_h = param_find("MPC_MAN_Y_MAX"); + _Pyaw_h = param_find("MC_YAW_P"); + /* Set parameter the very first time */ + _setParams(); }; -void TranslationControl::updateState(struct vehicle_local_position_s state) +void TranslationControl::updateState(const struct vehicle_local_position_s state, const matrix::Vector3f vel_dot, + const matrix::Matrix R) { _pos = Data(&state.x); _vel = Data(&state.vx); - _pos.print(); + _yaw = state.yaw; + _vel_dot = vel_dot; + _R = R; } void TranslationControl::updateSetpoint(struct vehicle_local_position_setpoint_s setpoint) @@ -61,6 +85,258 @@ void TranslationControl::updateSetpoint(struct vehicle_local_position_setpoint_s _pos_sp = Data(&setpoint.x); _vel_sp = Data(&setpoint.vx); _acc_sp = Data(&setpoint.acc_x); - _yaw_sp = setpoint.yaw; + _yaw_sp = setpoint.yaw; //integrate _yawspeed_sp = setpoint.yawspeed; + + _interfaceMapping(); +} + +void TranslationControl::generateThrustYawSetpoint(const float &dt) +{ + _updateParams(); + + _positionController(); + + _velocityController(dt); + + _yawController(dt); + +} + +void TranslationControl::_interfaceMapping() +{ + /* loop through x,y and z component */ + for (int i = 0; i <= 2; i++) { + + if (PX4_ISFINITE(_pos_sp(i))) { + + /* Check if velocity should be used as feedforward */ + if (!PX4_ISFINITE(_vel_sp(i))) { + _vel_sp(i) = 0.0f; + } + + } else { + + /* Position controller is not active + * -> Set setpoint just to current position since */ + _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 + */ + _vel_sp(i) = _vel(i); + } + } + } + + if (!PX4_ISFINITE(_yawspeed_sp)) { + _yawspeed_sp = 0.0f; + + if (!PX4_ISFINITE(_yaw_sp)) { + _yaw_sp = _yaw_sp_int; + } + + } else if (!PX4_ISFINITE(_yaw_sp)) { + _yaw_sp = _yaw_sp_int; + } +} + +/* generate desired velocity */ +void TranslationControl::_positionController() +{ + _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)); + + if (vel_norm_xy > _VelMaxXY) { + _vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_norm_xy; + _vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_norm_xy; + } + + _vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ[0], _VelMaxZ[1]); + + _velocityController(dt); +} + +/* generates desired thrust vector */ +void TranslationControl::_velocityController(const float &dt) +{ + /* velocity error */ + Data vel_err = _vel_sp - _vel; + + /* TODO: add offboard acceleration mode */ + matrix::Vector3f offset(0.0f, 0.0f, _ThrHover); + _thr_sp = Pv.emult(vel_err) + Dv.emult(_vel_dot) + _thr_int - offset; + + bool saturation_z, saturation_xy = false; + + /* limit minimum lift + * TODO: check if it is not better to limit magnitude + * of thrust vector + * TODO: case when thrust setpoint points down is not really considered + * */ + /* limit min lift */ + if (-_thr_sp(2) < _ThrLimit[1]) { + _thr_sp(2) = -_ThrLimit[1]; + /* Don't freeze altitude integral if it wants to throttle up */ + saturation_z = vel_err(2) > 0.0f ? true : saturation_z; + } + + /* limit max tilt */ + if (PX4_ISFINITE(_constraints.tilt_max)) { + /* limit max tilt */ + if (_ThrLimit[1] >= 0.0f && _constraints.tilt_max < M_PI_F / 2.0f - 0.05f) { + /* absolute horizontal thrust */ + float thrust_sp_xy_len = matrix::Vector2f(_thr_sp(0), _thr_sp(1)).length(); + + if (thrust_sp_xy_len > 0.01f) { + /* max horizontal thrust for given vertical thrust*/ + float thrust_xy_max = -_thr_sp(2) * tanf(_constraints.tilt_max); + + if (thrust_sp_xy_len > thrust_xy_max) { + float k = thrust_xy_max / thrust_sp_xy_len; + _thr_sp(0) *= k; + _thr_sp(1) *= k; + /* Don't freeze x,y integrals if they both want to throttle down */ + saturation_xy = + ((vel_err(0) * _vel_sp(0) < 0.0f) + && (vel_err(1) * _vel_sp(1) < 0.0f)) ? + saturation_xy : true; + } + } + } + } + + /* TODO: compensation logic if altitude is enabled: + * Check if it makes sense or not + */ + + /* Calculate desired total thrust amount in body z direction. */ + /* To compensate for excess thrust during attitude tracking errors we + * project the desired thrust force vector F onto the real vehicle's thrust axis in NED: + * body thrust axis [0,0,-1]' rotated by R is: R*[0,0,-1]' = -R_z */ + matrix::Vector3f R_z(_R(0, 2), _R(1, 2), _R(2, 2)); + _throttle = _thr_sp.dot(-R_z); /* recalculate because it might have changed */ + + + /* TODO: this logic seems very odd: check if it makes sense: + * possibly even add a control function library that deals with + * it and tests for it. + */ + /* limit max thrust */ + if (fabsf(_throttle) > _ThrLimit[0]) { + if (_thr_sp(2) < 0.0f) { + if (-_thr_sp(2) > _ThrLimit[0]) { + /* thrust Z component is too large, limit it */ + _thr_sp(0) = 0.0f; + _thr_sp(1) = 0.0f; + _thr_sp(2) = -_ThrLimit[0]; + saturation_xy = true; + /* Don't freeze altitude integral if it wants to throttle down */ + saturation_z = vel_err(2) < 0.0f ? true : saturation_z; + + } else { + /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ + float thrust_xy_max = sqrtf( + _ThrLimit[0] * _ThrLimit[0] - _thr_sp(2) * _thr_sp(2)); + float thrust_xy_abs = matrix::Vector2f(_thr_sp(0), + _thr_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + _thr_sp(0) *= k; + _thr_sp(1) *= k; + /* Don't freeze x,y integrals if they both want to throttle down */ + saturation_xy = + ((vel_err(0) * _vel_sp(0) < 0.0f) + && (vel_err(1) * _vel_sp(1) < 0.0f)) ? + saturation_xy : true; + } + + } else { + /* Z component is positive, going down (Z is positive down in NED), simply limit thrust vector */ + float k = _ThrLimit[0] / fabsf(_throttle); + _thr_sp *= k; + saturation_xy = true; + saturation_z = true; + } + + /* update integrals */ + if (!saturation_xy) { + _thr_int(0) += vel_err(0) * Iv(0) * dt; + _thr_int(1) += vel_err(1) * Iv(1) * dt; + } + + + /* TODO: check this entire logic + */ + _throttle = _ThrLimit[0]; + } +} + +void TranslationControl::_yawController(const float &dt) +{ + const float yaw_offset_max = _YawRateMax / _Pyaw; + const float yaw_target = _wrap_pi(_yaw_sp + _yawspeed_sp * dt); + const float yaw_offset = _wrap_pi(yaw_target - _yaw); + + // If the yaw offset became too big for the system to track stop + // shifting it, only allow if it would make the offset smaller again. + if (fabsf(yaw_offset) < yaw_offset_max || (_yawspeed_sp > 0 && yaw_offset < 0) + || (_yawspeed_sp < 0 && yaw_offset > 0)) { + _yaw_sp = yaw_target; + } + + /* update yaw setpoint integral */ + _yaw_sp_int = _yaw_sp; + +} + +void TranslationControl::updateConstraints(const Controller::Constraints &constraints) +{ + _constraints = constraints; +} + +void TranslationControl::_updateParams() +{ + bool updated; + parameter_update_s param_update; + orb_check(_parameter_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameter_sub, ¶m_update); + _setParams(); + } +} + +void TranslationControl::_setParams() +{ + param_get(_Pxy_h, &Pp(0)); + param_get(_Pxy_h, &Pp(1)); + param_get(_Pz_h, &Pp(2)); + + param_get(_Pvxy_h, &Pv(0)); + param_get(_Pvxy_h, &Pv(1)); + param_get(_Pvz_h, &Pv(2)); + + param_get(_Ivxy_h, &Iv(0)); + param_get(_Ivxy_h, &Iv(1)); + param_get(_Ivz_h, &Iv(2)); + + param_get(_Dvxy_h, &Dv(0)); + param_get(_Dvxy_h, &Dv(1)); + param_get(_Dvz_h, &Dv(2)); + + param_get(_VelMaxXY_h, &_VelMaxXY); + param_get(_VelMaxZup_h, &_VelMaxZ[0]); + param_get(_VelMaxZdown_h, &_VelMaxZ[1]); + + param_get(_ThrHover_h, &_ThrHover); + param_get(_ThrMax_h, &_ThrLimit[0]); + param_get(_ThrMin_h, &_ThrLimit[1]); + + param_get(_YawRateMax_h, &_YawRateMax); + param_get(_Pyaw_h, &_Pyaw); } diff --git a/src/modules/mc_pos_control/TranslationControl.hpp b/src/modules/mc_pos_control/TranslationControl.hpp index 2893b6ce76..396d46f20f 100644 --- a/src/modules/mc_pos_control/TranslationControl.hpp +++ b/src/modules/mc_pos_control/TranslationControl.hpp @@ -43,9 +43,20 @@ #include #include +#include #pragma once +/* Variable constraints based on mode: + * Eventually this structure should be part of local position message + */ +namespace Controller +{ +struct Constraints { + float tilt_max; +}; +} + class TranslationControl { public: @@ -53,21 +64,74 @@ public: ~TranslationControl() {}; - void updateState(struct vehicle_local_position_s state); + void updateState(const struct vehicle_local_position_s state, const matrix::Vector3f vel_dot, + const matrix::Matrix R); void updateSetpoint(struct vehicle_local_position_setpoint_s setpoint); + void updateConstraints(const Controller::Constraints &constraints); + void generateThrustYawSetpoint(const float &dt); + + matrix::Vector3f getThrustSetpoint() {return _thr_sp;} + float getYawSetpoint() { return _yaw_sp;} + float getThrottle() {return _throttle;} private: + /* states */ matrix::Vector3f _pos{0.0f, 0.0f, 0.0f}; matrix::Vector3f _vel{0.0f, 0.0f, 0.0f}; + matrix::Vector3f _vel_dot{0.0f, 0.0f, 0.0f}; matrix::Vector3f _acc{0.0f, 0.0f, 0.0f}; matrix::Vector3f _thr{0.0f, 0.0f, 0.0f}; float _yaw{0.0f}; + matrix::Matrix _R; + /* setpoints */ matrix::Vector3f _pos_sp{0.0f, 0.0f, 0.0f}; matrix::Vector3f _vel_sp{0.0f, 0.0f, 0.0f}; matrix::Vector3f _acc_sp{0.0f, 0.0f, 0.0f}; matrix::Vector3f _thr_sp{0.0f, 0.0f, 0.0f}; float _yaw_sp{0.0f}; float _yawspeed_sp{0.0f}; + float _throttle{}; + + /* other variables */ + matrix::Vector3f _thr_int{}; + float _yaw_sp_int{}; + Controller::Constraints _constraints{}; + + /* params 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 _ThrMin_h{PARAM_INVALID}; + param_t _YawRateMax_h{PARAM_INVALID}; + param_t _Pyaw_h{PARAM_INVALID}; //only temporary: this will be moved into attitude controller + + /* params */ + matrix::Vector3f Pp, Pv, Iv, Dv = matrix::Vector3f{0.0f, 0.0f, 0.0f}; + float _VelMaxXY{0.0f}; + float _VelMaxZ[2] = {0.0f, 0.0f}; //index 0: index up; 1: down + float _ThrHover{0.5f}; + float _ThrLimit[2] = {0.0f, 0.0f}; + float _Pyaw{}; + float _YawRateMax{}; + + /* helper methods */ + void _interfaceMapping(); + void _positionController(); + void _velocityController(const float &dt); + void _yawController(const float &dt); + void _updateParams(); + void _setParams(); };