Skip to content

Commit

Permalink
PositionControl: polish naming, order and comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@MaEtUgR
MaEtUgR committed Jun 2, 2019
1 parent cc6d02f commit 9e86cb1
Show file tree
Hide file tree
Showing 3 changed files with 73 additions and 86 deletions.
87 changes: 45 additions & 42 deletions src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,15 +47,15 @@ PositionControl::PositionControl(ModuleParams *parent) :
ModuleParams(parent)
{}

void PositionControl::updateState(const PositionControlStates &states)
void PositionControl::setState(const PositionControlStates &states)
{
_pos = states.position;
_vel = states.velocity;
_yaw = states.yaw;
_vel_dot = states.acceleration;
}

void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
{
_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
Expand All @@ -65,7 +65,32 @@ void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se
_yawspeed_sp = setpoint.yawspeed;
}

void PositionControl::generateThrustYawSetpoint(const float dt)
void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
{
_constraints = 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.

if (!PX4_ISFINITE(constraints.tilt)
|| !(constraints.tilt < math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()))) {
_constraints.tilt = math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get());
}

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_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_xy) || !(constraints.speed_xy < _param_mpc_xy_vel_max.get())) {
_constraints.speed_xy = _param_mpc_xy_vel_max.get();
}
}

void PositionControl::update(const float dt)
{
_positionController();
_velocityController(dt);
Expand All @@ -79,7 +104,7 @@ void PositionControl::_positionController()
// P-position controller
const Vector3f propotional_gain(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get());
Vector3f vel_sp_position = (_pos_sp - _pos).emult(propotional_gain);
addIfNotNanVector(_vel_sp, vel_sp_position);
_addIfNotNanVector(_vel_sp, vel_sp_position);

// Constrain horizontal velocity by prioritizing the velocity component along the
// the desired position setpoint over the feed-forward term.
Expand Down Expand Up @@ -121,15 +146,15 @@ void PositionControl::_velocityController(const float &dt)

const Vector3f vel_err = _vel_sp - _vel;

// Acceleration to thrust feedforward conversion
// Acceleration to thrust addition conversion
static constexpr float CONSTANTS_ONE_G = 9.80665f; // m/s^2
const float scale = CONSTANTS_ONE_G / _param_mpc_thr_hover.get();
const Vector3f thr_ff = _acc_sp * _param_mpc_thr_hover.get() / CONSTANTS_ONE_G;

// 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) + _vel_int(
2) - _param_mpc_thr_hover.get();
addIfNotNan(thrust_desired_D, thr_ff(2));
_addIfNotNan(thrust_desired_D, thr_ff(2));

acc_sp_velocity(2) += scale * (_param_mpc_z_vel_p.get() * vel_err(2) + _param_mpc_z_vel_d.get() * _vel_dot(2) + _vel_int(
2));
Expand Down Expand Up @@ -167,8 +192,8 @@ void PositionControl::_velocityController(const float &dt)
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) + _vel_int(0);
thrust_desired_NE(1) = _param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _vel_int(1);
addIfNotNan(thrust_desired_NE(0), thr_ff(0));
addIfNotNan(thrust_desired_NE(1), thr_ff(1));
_addIfNotNan(thrust_desired_NE(0), thr_ff(0));
_addIfNotNan(thrust_desired_NE(1), thr_ff(1));

acc_sp_velocity(0) += scale * (_param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _vel_int(0));
acc_sp_velocity(1) += scale * (_param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _vel_int(1));
Expand Down Expand Up @@ -202,9 +227,9 @@ void PositionControl::_velocityController(const float &dt)
}

// No control input from setpoints or corresponding states which are NAN, reset integrator if necessary
addIfNotNanVector(_vel_int, Vector3f());
addIfNotNanVector(_thr_sp, thr_sp_velocity);
addIfNotNanVector(_acc_sp, acc_sp_velocity);
_addIfNotNanVector(_vel_int, Vector3f());
_addIfNotNanVector(_thr_sp, thr_sp_velocity);
_addIfNotNanVector(_acc_sp, acc_sp_velocity);
}

void PositionControl::getOutputSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint)
Expand All @@ -221,31 +246,6 @@ void PositionControl::getOutputSetpoint(vehicle_local_position_setpoint_s &local
_thr_sp.copyTo(local_position_setpoint.thrust);
}

void PositionControl::updateConstraints(const vehicle_constraints_s &constraints)
{
_constraints = 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.

if (!PX4_ISFINITE(constraints.tilt)
|| !(constraints.tilt < math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()))) {
_constraints.tilt = math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get());
}

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_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_xy) || !(constraints.speed_xy < _param_mpc_xy_vel_max.get())) {
_constraints.speed_xy = _param_mpc_xy_vel_max.get();
}
}

void PositionControl::updateParams()
{
ModuleParams::updateParams();
Expand All @@ -255,19 +255,22 @@ void PositionControl::updateParams()
_param_mpc_man_tilt_max.set(math::radians(_param_mpc_man_tilt_max.get()));
}

void PositionControl::addIfNotNan(float &setpoint, const float feedforward)
void PositionControl::_addIfNotNan(float &setpoint, const float addition)
{
if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(feedforward)) {
setpoint += feedforward;
if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(addition)) {
// No NAN, add to the setpoint
setpoint += addition;

} else if (!PX4_ISFINITE(setpoint)) {
setpoint = feedforward;
// Setpoint NAN, take addition
setpoint = addition;
}
// Addition is NAN or both are NAN, nothing to do
}

void PositionControl::addIfNotNanVector(Vector3f &setpoint, const Vector3f &feedforward)
void PositionControl::_addIfNotNanVector(Vector3f &setpoint, const Vector3f &addition)
{
for (int i = 0; i < 3; i++) {
addIfNotNan(setpoint(i), feedforward(i));
_addIfNotNan(setpoint(i), addition(i));
}
}
54 changes: 21 additions & 33 deletions src/modules/mc_pos_control/PositionControl/PositionControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,53 +80,38 @@ class PositionControl : public ModuleParams
~PositionControl() = default;

/**
* Overwrites certain parameters.
* Overwrites are required for unit-conversion.
* This method should only be called if parameters
* have been updated.
*/
void overwriteParams();

/**
* Update the current vehicle state.
* Pass the current vehicle state to the controller
* @param PositionControlStates structure
*/
void updateState(const PositionControlStates &states);
void setState(const PositionControlStates &states);

/**
* Update the desired setpoints.
* Pass the desired setpoints
* @param setpoint a vehicle_local_position_setpoint_s structure
* @return true if setpoint has updated correctly
*/
void updateSetpoint(const vehicle_local_position_setpoint_s &setpoint);
void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint);

/**
* Set constraints that are stricter than the global limits.
* Pass constraints that are stricter than the global limits
* @param constraints a PositionControl structure with supported constraints
*/
void updateConstraints(const vehicle_constraints_s &constraints);
void setConstraints(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
* @param dt time in seconds since last iteration
*/
void generateThrustYawSetpoint(const float dt);
void update(const float dt);

/**
* Set the integral term in xy to 0.
* @see _thr_int
*/
void resetIntegralXY() { _vel_int(0) = _vel_int(1) = 0.0f; }

/**
* Set the integral term in z to 0.
* @see _thr_int
*/
void resetIntegralZ() { _vel_int(2) = 0.0f; }
void resetIntegral() { _vel_int = matrix::Vector3f(); }

/**
* Get the controllers output local position setpoint
Expand All @@ -143,22 +128,25 @@ class PositionControl : public ModuleParams
private:
void _positionController(); /** applies the P-position-controller */
void _velocityController(const float &dt); /** applies the PID-velocity-controller */
void addIfNotNan(float &setpoint, const float feedforward);
void addIfNotNanVector(matrix::Vector3f &setpoint, const matrix::Vector3f &feedforward);
void _addIfNotNan(float &setpoint, const float addition); /** adds to the setpoint but handles NAN cases correctly */
void _addIfNotNanVector(matrix::Vector3f &setpoint, const matrix::Vector3f &addition); /** _addIfNotNan for all vector components */

matrix::Vector3f _pos; /**< MC position */
matrix::Vector3f _vel; /**< MC velocity */
matrix::Vector3f _vel_dot; /**< MC velocity derivative */
// States
matrix::Vector3f _pos; /**< position */
matrix::Vector3f _vel; /**< velocity */
matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */
matrix::Vector3f _vel_int; /**< integral term of the velocity controller */
matrix::Vector3f _acc; /**< MC acceleration */
float _yaw = 0.0f; /**< MC yaw */
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: not supported yet */
matrix::Vector3f _acc_sp; /**< desired acceleration */
matrix::Vector3f _thr_sp; /**< desired thrust */
float _yaw_sp{}; /**< desired yaw */
float _yawspeed_sp{}; /** desired yaw-speed */
vehicle_constraints_s _constraints{}; /**< variable constraints */

DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max,
Expand Down
18 changes: 7 additions & 11 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -645,8 +645,7 @@ MulticopterPositionControl::run()
setpoint.yaw = _states.yaw;
setpoint.yawspeed = 0.f;
// prevent any integrator windup
_control.resetIntegralXY();
_control.resetIntegralZ();
_control.resetIntegral();
// reactivate the task which will reset the setpoint to current state
_flight_tasks.reActivate();
}
Expand All @@ -664,8 +663,7 @@ MulticopterPositionControl::run()
setpoint.yaw = _states.yaw;
setpoint.yawspeed = 0.f;
// prevent any integrator windup
_control.resetIntegralXY();
_control.resetIntegralZ();
_control.resetIntegral();
}
}

Expand All @@ -679,15 +677,13 @@ MulticopterPositionControl::run()
limit_altitude(setpoint);
}

// Set position controller input
_control.updateConstraints(constraints);
_control.updateState(_states);
_control.updateSetpoint(setpoint);

// Run position control
_control.generateThrustYawSetpoint(_dt);
_control.setState(_states);
_control.setInputSetpoint(setpoint);
_control.setConstraints(constraints);
_control.update(_dt);

// Get position controller output
// Get position control output
vehicle_local_position_setpoint_s local_pos_sp{};
local_pos_sp.timestamp = hrt_absolute_time();
_control.getOutputSetpoint(local_pos_sp);
Expand Down

0 comments on commit 9e86cb1

Please sign in to comment.