diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 3db656e877..282cf330c5 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -43,9 +43,25 @@ using namespace matrix; -PositionControl::PositionControl(ModuleParams *parent) : - ModuleParams(parent) -{} +void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +{ + _gain_vel_p = P; + _gain_vel_i = I; + _gain_vel_d = D; +} + +void PositionControl::setVelocityLimits(const float vel_horizontal, const float vel_up, const float vel_down) +{ + _lim_vel_horizontal = vel_horizontal; + _lim_vel_up = vel_up; + _lim_vel_down = vel_down; +} + +void PositionControl::setThrustLimits(const float min, const float max) +{ + _lim_thr_min = min; + _lim_thr_max = max; +} void PositionControl::updateState(const PositionControlStates &states) { @@ -91,17 +107,17 @@ void PositionControl::generateThrustYawSetpoint(const float dt) // Limit the thrust vector. float thr_mag = _thr_sp.length(); - if (thr_mag > _param_mpc_thr_max.get()) { - _thr_sp = _thr_sp.normalized() * _param_mpc_thr_max.get(); + if (thr_mag > _lim_thr_max) { + _thr_sp = _thr_sp.normalized() * _lim_thr_max; - } else if (thr_mag < _param_mpc_manthr_min.get() && thr_mag > FLT_EPSILON) { - _thr_sp = _thr_sp.normalized() * _param_mpc_manthr_min.get(); + } else if (thr_mag < _lim_thr_min && thr_mag > FLT_EPSILON) { + _thr_sp = _thr_sp.normalized() * _lim_thr_min; } // Just set the set-points equal to the current vehicle state. _pos_sp = _pos; _vel_sp = _vel; - _acc_sp = _acc; + _acc_sp = _vel_dot; } else { _positionController(); @@ -204,7 +220,7 @@ bool PositionControl::_interfaceMapping() _thr_sp(0) = _thr_sp(1) = 0.0f; // throttle down such that vehicle goes down with // 70% of throttle range between min and hover - _thr_sp(2) = -(_param_mpc_thr_min.get() + (_param_mpc_thr_hover.get() - _param_mpc_thr_min.get()) * 0.7f); + _thr_sp(2) = -(_lim_thr_min + (_hover_thrust - _lim_thr_min) * 0.7f); // position and velocity control-loop is currently unused (flag only for logging purpose) _setCtrlFlag(false); } @@ -215,14 +231,13 @@ bool PositionControl::_interfaceMapping() void PositionControl::_positionController() { // P-position controller - const Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), - _param_mpc_z_p.get())); + const Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); _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. const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position), - Vector2f(_vel_sp - vel_sp_position), _param_mpc_xy_vel_max.get()); + Vector2f(_vel_sp - vel_sp_position), _lim_vel_horizontal); _vel_sp(0) = vel_sp_xy(0); _vel_sp(1) = vel_sp_xy(1); // Constrain velocity in z-direction. @@ -258,12 +273,12 @@ void PositionControl::_velocityController(const float &dt) const Vector3f vel_err = _vel_sp - _vel; // Consider thrust in D-direction. - float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) + _param_mpc_z_vel_d.get() * _vel_dot(2) + _thr_int( - 2) - _param_mpc_thr_hover.get(); + float thrust_desired_D = _gain_vel_p(2) * vel_err(2) + _gain_vel_d(2) * _vel_dot(2) + _thr_int( + 2) - _hover_thrust; // The Thrust limits are negated and swapped due to NED-frame. - float uMax = -_param_mpc_thr_min.get(); - float uMin = -_param_mpc_thr_max.get(); + float uMax = -_lim_thr_min; + float uMin = -_lim_thr_max; // make sure there's always enough thrust vector length to infer the attitude uMax = math::min(uMax, -10e-4f); @@ -273,14 +288,15 @@ void PositionControl::_velocityController(const float &dt) (thrust_desired_D <= uMin && vel_err(2) <= 0.0f); if (!stop_integral_D) { - _thr_int(2) += vel_err(2) * _param_mpc_z_vel_i.get() * dt; + _thr_int(2) += vel_err(2) * _gain_vel_i(2) * dt; // limit thrust integral - _thr_int(2) = math::min(fabsf(_thr_int(2)), _param_mpc_thr_max.get()) * math::sign(_thr_int(2)); + _thr_int(2) = math::min(fabsf(_thr_int(2)), _lim_thr_max) * math::sign(_thr_int(2)); } // Saturate thrust setpoint in D-direction. _thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax); + printf("%.3f %.3f\n", (double)uMin, (double)uMax); if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) { // Thrust set-point in NE-direction is already provided. Only @@ -292,12 +308,13 @@ void PositionControl::_velocityController(const float &dt) } else { // PID-velocity controller for NE-direction. Vector2f thrust_desired_NE; - thrust_desired_NE(0) = _param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _thr_int(0); - thrust_desired_NE(1) = _param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _thr_int(1); + thrust_desired_NE(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _thr_int(0); + thrust_desired_NE(1) = _gain_vel_p(1) * vel_err(1) + _gain_vel_d(1) * _vel_dot(1) + _thr_int(1); // Get maximum allowed thrust in NE based on tilt and excess thrust. float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt); - float thrust_max_NE = sqrtf(_param_mpc_thr_max.get() * _param_mpc_thr_max.get() - _thr_sp(2) * _thr_sp(2)); + float thrust_max_NE = sqrtf(_lim_thr_max * _lim_thr_max - _thr_sp(2) * _thr_sp(2)); + printf("%.3f %.3f\n", (double)(_lim_thr_max), (double)(_thr_sp(2))); thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE); // Saturate thrust in NE-direction. @@ -312,15 +329,15 @@ void PositionControl::_velocityController(const float &dt) // Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 - float arw_gain = 2.f / _param_mpc_xy_vel_p.get(); + float arw_gain = 2.f / _gain_vel_p(0); Vector2f vel_err_lim; vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain; vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain; // Update integral - _thr_int(0) += _param_mpc_xy_vel_i.get() * vel_err_lim(0) * dt; - _thr_int(1) += _param_mpc_xy_vel_i.get() * vel_err_lim(1) * dt; + _thr_int(0) += _gain_vel_i(0) * vel_err_lim(0) * dt; + _thr_int(1) += _gain_vel_i(1) * vel_err_lim(1) * dt; } } @@ -331,23 +348,21 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints // For safety check if adjustable constraints are below global constraints. If they are not stricter than global // constraints, then just use global constraints for the limits. - const float tilt_max_radians = math::radians(math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get())); - if (!PX4_ISFINITE(constraints.tilt) - || !(constraints.tilt < tilt_max_radians)) { - _constraints.tilt = tilt_max_radians; + || !(constraints.tilt < _lim_tilt)) { + _constraints.tilt = _lim_tilt; } - if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _param_mpc_z_vel_max_up.get())) { - _constraints.speed_up = _param_mpc_z_vel_max_up.get(); + if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _lim_vel_up)) { + _constraints.speed_up = _lim_vel_up; } - if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < _param_mpc_z_vel_max_dn.get())) { - _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); + if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < _lim_vel_down)) { + _constraints.speed_down = _lim_vel_down; } - if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < _param_mpc_xy_vel_max.get())) { - _constraints.speed_xy = _param_mpc_xy_vel_max.get(); + if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < _lim_vel_horizontal)) { + _constraints.speed_xy = _lim_vel_horizontal; } } @@ -370,8 +385,3 @@ void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_ ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint); attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; } - -void PositionControl::updateParams() -{ - ModuleParams::updateParams(); -} diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index c881762660..6511742721 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -37,14 +37,13 @@ * A cascaded position controller for position/velocity control only. */ -#include +#pragma once +#include #include +#include #include #include -#include -#include -#pragma once struct PositionControlStates { matrix::Vector3f position; @@ -73,20 +72,53 @@ struct PositionControlStates { * 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 ModuleParams +class PositionControl { public: - PositionControl(ModuleParams *parent); + PositionControl() = default; ~PositionControl() = default; /** - * Overwrites certain parameters. - * Overwrites are required for unit-conversion. - * This method should only be called if parameters - * have been updated. + * Set the position control gains + * @param P 3D vector of proportional gains for x,y,z axis */ - void overwriteParams(); + 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. @@ -166,10 +198,6 @@ public: */ void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const; -protected: - - void updateParams() override; - private: /** * Maps setpoints to internal-setpoints. @@ -181,42 +209,40 @@ private: void _velocityController(const float &dt); /** applies the PID-velocity-controller */ void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */ - 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 */ - 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 */ + // 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 */ - matrix::Vector3f _thr_int{}; /**< thrust integral term */ - vehicle_constraints_s _constraints{}; /**< variable constraints */ + 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 */ - - DEFINE_PARAMETERS( - (ParamFloat) _param_mpc_thr_max, - (ParamFloat) _param_mpc_thr_hover, - (ParamFloat) _param_mpc_thr_min, - (ParamFloat) _param_mpc_manthr_min, - (ParamFloat) _param_mpc_xy_vel_max, - (ParamFloat) _param_mpc_z_vel_max_dn, - (ParamFloat) _param_mpc_z_vel_max_up, - (ParamFloat) - _param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in degrees - (ParamFloat) - _param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in degrees - (ParamFloat) _param_mpc_z_p, - (ParamFloat) _param_mpc_z_vel_p, - (ParamFloat) _param_mpc_z_vel_i, - (ParamFloat) _param_mpc_z_vel_d, - (ParamFloat) _param_mpc_xy_p, - (ParamFloat) _param_mpc_xy_vel_p, - (ParamFloat) _param_mpc_xy_vel_i, - (ParamFloat) _param_mpc_xy_vel_d - ) }; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e9709087c0..2c6c5a8081 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -146,24 +146,36 @@ private: int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP}; DEFINE_PARAMETERS( - (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ + // Position Control + (ParamFloat) _param_mpc_xy_p, + (ParamFloat) _param_mpc_z_p, + (ParamFloat) _param_mpc_xy_vel_p, + (ParamFloat) _param_mpc_xy_vel_i, + (ParamFloat) _param_mpc_xy_vel_d, + (ParamFloat) _param_mpc_z_vel_p, + (ParamFloat) _param_mpc_z_vel_i, + (ParamFloat) _param_mpc_z_vel_d, (ParamFloat) _param_mpc_xy_vel_max, - (ParamFloat) _param_mpc_vel_manual, - (ParamFloat) _param_mpc_xy_cruise, (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) _param_mpc_z_vel_max_dn, - (ParamFloat) _param_mpc_land_speed, + (ParamFloat) _param_mpc_tiltmax_air, + (ParamFloat) _param_mpc_thr_hover, + + // Takeoff / Land + (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ + (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ (ParamFloat) _param_mpc_tko_speed, + (ParamFloat) _param_mpc_land_speed, + + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_xy_cruise, (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ (ParamInt) _param_mpc_pos_mode, (ParamInt) _param_mpc_auto_mode, (ParamInt) _param_mpc_alt_mode, - (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ - (ParamFloat)_param_mpc_thr_min, - (ParamFloat)_param_mpc_thr_hover, - (ParamFloat)_param_mpc_thr_max, - (ParamFloat)_param_mpc_z_vel_p + (ParamFloat) _param_mpc_thr_min, + (ParamFloat) _param_mpc_thr_max ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ @@ -275,7 +287,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_x_deriv(this, "VELD"), _vel_y_deriv(this, "VELD"), _vel_z_deriv(this, "VELD"), - _control(this), _cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time")) { // fetch initial parameter values @@ -331,6 +342,15 @@ MulticopterPositionControl::parameters_update(bool force) ModuleParams::updateParams(); SuperBlock::updateParams(); + _control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get())); + _control.setVelocityGains(Vector3f(_param_mpc_xy_vel_p.get(), _param_mpc_xy_vel_p.get(), _param_mpc_z_vel_p.get()), + Vector3f(_param_mpc_xy_vel_i.get(), _param_mpc_xy_vel_i.get(), _param_mpc_z_vel_i.get()), + Vector3f(_param_mpc_xy_vel_d.get(), _param_mpc_xy_vel_d.get(), _param_mpc_z_vel_d.get())); + _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); + _control.setThrustLimits(_param_mpc_thr_min.get(), _param_mpc_thr_max.get()); + _control.setTiltLimit(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()); // convert to radians! + _control.setHoverThrust(_param_mpc_thr_hover.get()); + // Check that the design parameters are inside the absolute maximum constraints if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) { _param_mpc_xy_cruise.set(_param_mpc_xy_vel_max.get());