Skip to content

Commit

Permalink
GotoControl: simplify configuration wrapping
Browse files Browse the repository at this point in the history
  • Loading branch information
@MaEtUgR
MaEtUgR committed Nov 23, 2023
1 parent 8a9bb9f commit d681e30
Show file tree
Hide file tree
Showing 4 changed files with 51 additions and 97 deletions.
46 changes: 26 additions & 20 deletions src/modules/mc_pos_control/GotoControl/GotoControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,24 +131,30 @@ void GotoControl::resetHeadingSmoother(const float heading)
_heading_smoothing.reset(heading, initial_heading_rate);
}

void GotoControl::updateParams()
{
ModuleParams::updateParams();
setMaxAllowedHorizontalPositionError(_param_mpc_xy_err_max.get());
}

void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint)
{
// constrain horizontal velocity
float max_horizontal_speed = _goto_constraints.max_horizontal_speed;
float max_horizontal_accel = _goto_constraints.max_horizontal_accel;
float max_horizontal_speed = _param_mpc_xy_cruise.get();
float max_horizontal_accel = _param_mpc_acc_hor.get();

if (goto_setpoint.flag_set_max_horizontal_speed
&& PX4_ISFINITE(goto_setpoint.max_horizontal_speed)) {
max_horizontal_speed = math::constrain(goto_setpoint.max_horizontal_speed, 0.f,
_goto_constraints.max_horizontal_speed);
_param_mpc_xy_cruise.get());

// linearly scale horizontal acceleration limit with horizontal speed limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints
if (_position_smoothing.getCurrentVelocityXY().norm() <= max_horizontal_speed) {
const float speed_scale = max_horizontal_speed / _goto_constraints.max_horizontal_speed;
max_horizontal_accel = math::constrain(_goto_constraints.max_horizontal_accel * speed_scale,
const float speed_scale = max_horizontal_speed / _param_mpc_xy_cruise.get();
max_horizontal_accel = math::constrain(_param_mpc_acc_hor.get() * speed_scale,
0.f,
_goto_constraints.max_horizontal_accel);
_param_mpc_acc_hor.get());
}
}

Expand All @@ -159,10 +165,10 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint
// constrain vertical velocity
const bool pos_setpoint_is_below_smooth_pos = goto_setpoint.position[2] - _position_smoothing.getCurrentPositionZ() >
0.f;
const float vehicle_max_vertical_speed = (pos_setpoint_is_below_smooth_pos) ? _goto_constraints.max_down_speed :
_goto_constraints.max_up_speed;
const float vehicle_max_vertical_accel = (pos_setpoint_is_below_smooth_pos) ? _goto_constraints.max_down_accel :
_goto_constraints.max_up_accel;
const float vehicle_max_vertical_speed = (pos_setpoint_is_below_smooth_pos) ? _param_mpc_z_v_auto_dn.get() :
_param_mpc_z_v_auto_up.get();
const float vehicle_max_vertical_accel = (pos_setpoint_is_below_smooth_pos) ? _param_mpc_acc_down_max.get() :
_param_mpc_acc_up_max.get();

float max_vertical_speed = vehicle_max_vertical_speed;
float max_vertical_accel = vehicle_max_vertical_accel;
Expand All @@ -183,25 +189,25 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint
_position_smoothing.setMaxVelocityZ(max_vertical_speed);
_position_smoothing.setMaxAccelerationZ(max_vertical_accel);

_position_smoothing.setMaxJerkXY(_goto_constraints.max_jerk);
_position_smoothing.setMaxJerkZ(_goto_constraints.max_jerk);
_position_smoothing.setMaxJerkXY(_param_mpc_jerk_auto.get());
_position_smoothing.setMaxJerkZ(_param_mpc_jerk_auto.get());
}

void GotoControl::setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint)
{
float max_heading_rate = _goto_constraints.max_heading_rate;
float max_heading_accel = _goto_constraints.max_heading_accel;
float max_heading_rate = _param_mpc_yawrauto_max.get();
float max_heading_accel = _param_mpc_yawaauto_max.get();

if (goto_setpoint.flag_set_max_heading_rate && PX4_ISFINITE(goto_setpoint.max_heading_rate)) {
max_heading_rate = math::constrain(goto_setpoint.max_heading_rate, 0.f,
_goto_constraints.max_heading_rate);
if (goto_setpoint.flag_set_max_heading_rate && PX4_ISFINITE(_param_mpc_yawrauto_max.get())) {
max_heading_rate = math::constrain(_param_mpc_yawrauto_max.get(), 0.f,
_param_mpc_yawrauto_max.get());

// linearly scale heading acceleration limit with heading rate limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints
if (fabsf(_heading_smoothing.getSmoothedHeadingRate()) <= max_heading_rate) {
const float rate_scale = max_heading_rate / _goto_constraints.max_heading_rate;
max_heading_accel = math::constrain(_goto_constraints.max_heading_accel * rate_scale,
0.f, _goto_constraints.max_heading_accel);
const float rate_scale = max_heading_rate / _param_mpc_yawrauto_max.get();
max_heading_accel = math::constrain(_param_mpc_yawaauto_max.get() * rate_scale,
0.f, _param_mpc_yawaauto_max.get());
}
}

Expand Down
59 changes: 20 additions & 39 deletions src/modules/mc_pos_control/GotoControl/GotoControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,50 +46,16 @@
#include <lib/motion_planning/PositionSmoothing.hpp>
#include <mathlib/math/Limits.hpp>
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/topics/goto_setpoint.h>
#include <uORB/topics/trajectory_setpoint.h>

class GotoControl
class GotoControl : public ModuleParams
{
public:
GotoControl() = default;
GotoControl(ModuleParams *parent) : ModuleParams(parent) {};
~GotoControl() = default;

/** @brief struct containing maximum vehicle translational and rotational constraints */
struct GotoConstraints {
float max_horizontal_speed; // [m/s]
float max_down_speed; // [m/s]
float max_up_speed; // [m/s]
float max_horizontal_accel; // [m/s^2]
float max_down_accel; // [m/s^2]
float max_up_accel; // [m/s^2]
float max_jerk; // [m/s^3]
float max_heading_rate; // [rad/s]
float max_heading_accel; // [rad/s^2]
};

/**
* @brief sets the maximum vehicle translational and rotational constraints. note these can be more conservatively
* overriden (e.g. slowed down) via the speed scalers in the go-to setpoint.
*
* @param vehicle_constraints Struct containing desired vehicle constraints
*/
void setGotoConstraints(const GotoConstraints &vehicle_constraints)
{
_goto_constraints.max_horizontal_speed = math::max(0.f, vehicle_constraints.max_horizontal_speed);
_goto_constraints.max_down_speed = math::max(0.f, vehicle_constraints.max_down_speed);
_goto_constraints.max_up_speed = math::max(0.f, vehicle_constraints.max_up_speed);
_goto_constraints.max_horizontal_accel = math::max(0.f,
vehicle_constraints.max_horizontal_accel);
_goto_constraints.max_down_accel = math::max(0.f, vehicle_constraints.max_down_accel);
_goto_constraints.max_up_accel = math::max(0.f, vehicle_constraints.max_up_accel);
_goto_constraints.max_jerk = math::max(0.f, vehicle_constraints.max_jerk);
_goto_constraints.max_heading_rate = math::max(0.f,
vehicle_constraints.max_heading_rate);
_goto_constraints.max_heading_accel = math::max(0.f,
vehicle_constraints.max_heading_accel);
}

/** @param error [m] position smoother's maximum allowed horizontal position error at which trajectory integration halts */
void setMaxAllowedHorizontalPositionError(const float error) { _position_smoothing.setMaxAllowedHorizontalError(error); }

Expand Down Expand Up @@ -120,7 +86,11 @@ class GotoControl
void update(const float dt, const matrix::Vector3f &position, const float heading,
const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint);

bool is_initialized{false};

private:
void updateParams() override;

/**
* @brief optionally sets dynamic translational speed limits with corresponding scale on acceleration
*
Expand All @@ -135,8 +105,6 @@ class GotoControl
*/
void setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint);

GotoConstraints _goto_constraints{};

PositionSmoothing _position_smoothing;
HeadingSmoothing _heading_smoothing;

Expand All @@ -145,4 +113,17 @@ class GotoControl

// flags if the last update() was controlling heading
bool _controlling_heading{false};

DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
(ParamFloat<px4::params::MPC_YAWAAUTO_MAX>) _param_mpc_yawaauto_max,
(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max
);
};
21 changes: 3 additions & 18 deletions src/modules/mc_pos_control/MulticopterPositionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -379,27 +379,13 @@ void MulticopterPositionControl::Run()
&& (_goto_setpoint.timestamp >= _time_position_control_enabled)
&& (hrt_elapsed_time(&last_goto_timestamp) < 200_ms)
&& _vehicle_control_mode.flag_multicopter_position_control_enabled) {
// take position heading setpoint as priority over trajectory setpoint
// take goto setpoint as priority over trajectory setpoint

if (_last_active_setpoint_interface == SetpointInterface::kTrajectory) {
if (!_goto_control.is_initialized) {
_goto_control.resetPositionSmoother(states.position);
_goto_control.resetHeadingSmoother(states.yaw);
}

const GotoControl::GotoConstraints goto_constraints = {
_param_mpc_xy_cruise.get(),
_param_mpc_z_v_auto_dn.get(),
_param_mpc_z_v_auto_up.get(),
_param_mpc_acc_hor.get(),
_param_mpc_acc_down_max.get(),
_param_mpc_acc_up_max.get(),
_param_mpc_jerk_auto.get(),
_param_mpc_yawrauto_max.get(),
_param_mpc_yawaauto_max.get()
};

_goto_control.setGotoConstraints(goto_constraints);
_goto_control.setMaxAllowedHorizontalPositionError(_param_mpc_xy_err_max.get());

_goto_control.update(dt, states.position, states.yaw, _goto_setpoint, _setpoint);

Expand All @@ -411,10 +397,9 @@ void MulticopterPositionControl::Run()
_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();
_vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get();

_last_active_setpoint_interface = SetpointInterface::kGoto;

} else {
_last_active_setpoint_interface = SetpointInterface::kTrajectory;
_goto_control.is_initialized = false;
_vehicle_constraints_sub.update(&_vehicle_constraints);
}

Expand Down
22 changes: 2 additions & 20 deletions src/modules/mc_pos_control/MulticopterPositionControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,12 +132,7 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
.landed = true,
};

enum SetpointInterface {
kTrajectory = 0,
kGoto
} _last_active_setpoint_interface{kTrajectory};

GotoControl _goto_control;
GotoControl _goto_control{this};

DEFINE_PARAMETERS(
// Position Control
Expand Down Expand Up @@ -188,11 +183,7 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau,

(ParamFloat<px4::params::MPC_XY_VEL_ALL>) _param_mpc_xy_vel_all,
(ParamFloat<px4::params::MPC_Z_VEL_ALL>) _param_mpc_z_vel_all,

(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
(ParamFloat<px4::params::MPC_YAWAAUTO_MAX>) _param_mpc_yawaauto_max,
(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max
(ParamFloat<px4::params::MPC_Z_VEL_ALL>) _param_mpc_z_vel_all
);

control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
Expand Down Expand Up @@ -241,13 +232,4 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
* This should only happen briefly when transitioning and never during mode operation or by design.
*/
trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn);

/**
* @brief adjust existing (or older) setpoint with any EKF reset deltas and update the local counters
*
* @param[in] vehicle_local_position struct containing EKF reset deltas and counters
* @param[out] setpoint trajectory setpoint struct to be adjusted
*/
void adjustSetpointForEKFResets(const vehicle_local_position_s &vehicle_local_position,
trajectory_setpoint_s &setpoint);
};

0 comments on commit d681e30

Please sign in to comment.