Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

MC pos control - Dynamically constrain parameters #12404

Merged
merged 4 commits into from
Jul 5, 2019
Merged

MC pos control - Dynamically constrain parameters #12404

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
1d43974
MC pos control - Force cruise and manual speeds below max speed. Force
bresch Jul 3, 2019
4f423cd
Merge branch 'master' into pr-mc-param-check
bresch Jul 4, 2019
c657325
Merge branch 'master' into pr-mc-param-check
bresch Jul 5, 2019
5ef5846
Merge branch 'master' into pr-mc-param-check
dagar Jul 5, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 27 additions & 0 deletions src/modules/mc_pos_control/mc_pos_control_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,7 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>

orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */
orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command publication */
orb_advert_t _mavlink_log_pub{nullptr};

orb_id_t _attitude_setpoint_id{nullptr};

Expand Down Expand Up @@ -147,6 +148,9 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>

DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
Expand All @@ -157,7 +161,9 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
(ParamFloat<px4::params::MPC_THR_MIN>)_param_mpc_thr_min,
(ParamFloat<px4::params::MPC_THR_HOVER>)_param_mpc_thr_hover,
(ParamFloat<px4::params::MPC_THR_MAX>)_param_mpc_thr_max,
(ParamFloat<px4::params::MPC_Z_VEL_P>)_param_mpc_z_vel_p
);

Expand Down Expand Up @@ -312,6 +318,27 @@ MulticopterPositionControl::parameters_update(bool force)
ModuleParams::updateParams();
SuperBlock::updateParams();

// 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());
_param_mpc_xy_cruise.commit();
mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed")
}

if (_param_mpc_vel_manual.get() > _param_mpc_xy_vel_max.get()) {
_param_mpc_vel_manual.set(_param_mpc_xy_vel_max.get());
_param_mpc_vel_manual.commit();
mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed")
}

if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() ||
_param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) {
_param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(),
_param_mpc_thr_max.get()));
_param_mpc_thr_hover.commit();
mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max")
}

_flight_tasks.handleParameterUpdate();

// initialize vectors from params and enforce constraints
Expand Down