From 96a81c22e3af17e1232fc30816e4e24e5cd9ab8b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 23 Nov 2023 12:03:42 +0100 Subject: [PATCH] GotoControl: simplify configuration wrapping --- .../GotoControl/GotoControl.cpp | 46 ++++++++------- .../GotoControl/GotoControl.hpp | 59 +++++++------------ .../MulticopterPositionControl.cpp | 23 +------- .../MulticopterPositionControl.hpp | 13 +--- 4 files changed, 51 insertions(+), 90 deletions(-) diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp index 4d08e359c4..2a811316c0 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp @@ -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()); } } @@ -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; @@ -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()); } } diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp index 202aee55b5..968c83669e 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp @@ -46,50 +46,16 @@ #include #include #include +#include #include #include -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); } @@ -120,7 +86,11 @@ public: 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 * @@ -135,8 +105,6 @@ private: */ void setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint); - GotoConstraints _goto_constraints{}; - PositionSmoothing _position_smoothing; HeadingSmoothing _heading_smoothing; @@ -145,4 +113,17 @@ private: // flags if the last update() was controlling heading bool _controlling_heading{false}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mpc_xy_cruise, + (ParamFloat) _param_mpc_z_v_auto_dn, + (ParamFloat) _param_mpc_z_v_auto_up, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_acc_down_max, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_yawrauto_max, + (ParamFloat) _param_mpc_yawaauto_max, + (ParamFloat) _param_mpc_xy_err_max + ); }; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index c400340457..6f3007c497 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -379,28 +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); // for logging @@ -411,10 +396,8 @@ 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); } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 6f7e73e5e9..a606eee290 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -132,12 +132,7 @@ private: .landed = true, }; - enum SetpointInterface { - kTrajectory = 0, - kGoto - } _last_active_setpoint_interface{kTrajectory}; - - GotoControl _goto_control; + GotoControl _goto_control{this}; DEFINE_PARAMETERS( // Position Control @@ -188,11 +183,7 @@ private: (ParamFloat) _param_mpc_man_y_tau, (ParamFloat) _param_mpc_xy_vel_all, - (ParamFloat) _param_mpc_z_vel_all, - - (ParamFloat) _param_mpc_yawrauto_max, - (ParamFloat) _param_mpc_yawaauto_max, - (ParamFloat) _param_mpc_xy_err_max + (ParamFloat) _param_mpc_z_vel_all ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */