fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 12:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

GotoControl: simplify configuration wrapping

Browse Source
This commit is contained in:
Matthias Grob
2023-11-30 17:16:02 +01:00
parent 439d6c61e0
commit 96a81c22e3
4 changed files with 51 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/GotoControl/GotoControl.cpp
+26 -20
View File
@@ -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());
}
}
src/modules/mc_pos_control/GotoControl/GotoControl.hpp
+20 -39
View File
@@ -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); }
@@ -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<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
);
};
src/modules/mc_pos_control/MulticopterPositionControl.cpp
+3 -20
View File
@@ -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);
}
src/modules/mc_pos_control/MulticopterPositionControl.hpp
+2 -11
View File
@@ -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<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 */