dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

GotoControl: also provide maximum velocity to position smoother

This commit is contained in:
Matthias Grob 2023-11-27 14:45:33 +01:00
parent d1e8bdbd16
commit c853acc2ff
2 changed files with 23 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/modules/mc_pos_control/GotoControl/GotoControl.cpp
View File

@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2019 PX4 Development Team. All rights reserved. * Copyright (c) 2023 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -172,7 +172,7 @@ void GotoControl::updateParams()
void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint) void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint)
{ {
// constrain horizontal velocity // Horizontal constraints
float max_horizontal_speed = _param_mpc_xy_cruise.get(); float max_horizontal_speed = _param_mpc_xy_cruise.get();
float max_horizontal_accel = _param_mpc_acc_hor.get(); float max_horizontal_accel = _param_mpc_acc_hor.get();
@ -183,46 +183,42 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint
// linearly scale horizontal acceleration limit with horizontal speed limit to maintain smoothing dynamic // linearly scale horizontal acceleration limit with horizontal speed limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints // only limit acceleration once within velocity constraints
if (_position_smoothing.getCurrentVelocityXY().norm() <= max_horizontal_speed) { if (!_position_smoothing.getCurrentVelocityXY().longerThan(max_horizontal_speed)) {
const float speed_scale = max_horizontal_speed / _param_mpc_xy_cruise.get(); const float speed_scale = max_horizontal_speed / _param_mpc_xy_cruise.get();
max_horizontal_accel = math::constrain(_param_mpc_acc_hor.get() * speed_scale, max_horizontal_accel = math::constrain(_param_mpc_acc_hor.get() * speed_scale, 0.f, _param_mpc_acc_hor.get());
0.f,
_param_mpc_acc_hor.get());
} }
} }
_position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get());
_position_smoothing.setCruiseSpeed(max_horizontal_speed); _position_smoothing.setCruiseSpeed(max_horizontal_speed);
_position_smoothing.setMaxVelocityXY(max_horizontal_speed);
_position_smoothing.setMaxAccelerationXY(max_horizontal_accel); _position_smoothing.setMaxAccelerationXY(max_horizontal_accel);
_position_smoothing.setMaxJerkXY(_param_mpc_jerk_auto.get());
// constrain vertical velocity // Vertical constraints
const bool pos_setpoint_is_below_smooth_pos = goto_setpoint.position[2] - _position_smoothing.getCurrentPositionZ() > float vehicle_max_vertical_speed = _param_mpc_z_v_auto_dn.get();
0.f; float vehicle_max_vertical_accel = _param_mpc_acc_down_max.get();
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(); if (goto_setpoint.position[2] < _position_smoothing.getCurrentPositionZ()) { // goto higher -> more negative
const float vehicle_max_vertical_accel = (pos_setpoint_is_below_smooth_pos) ? _param_mpc_acc_down_max.get() : vehicle_max_vertical_speed = _param_mpc_z_v_auto_up.get();
_param_mpc_acc_up_max.get(); vehicle_max_vertical_accel = _param_mpc_acc_up_max.get();
}
float max_vertical_speed = vehicle_max_vertical_speed; float max_vertical_speed = vehicle_max_vertical_speed;
float max_vertical_accel = vehicle_max_vertical_accel; float max_vertical_accel = vehicle_max_vertical_accel;
if (goto_setpoint.flag_set_max_vertical_speed && PX4_ISFINITE(goto_setpoint.max_vertical_speed)) { if (goto_setpoint.flag_set_max_vertical_speed && PX4_ISFINITE(goto_setpoint.max_vertical_speed)) {
max_vertical_speed = math::constrain(goto_setpoint.max_vertical_speed, 0.f, vehicle_max_vertical_speed); max_vertical_speed = math::constrain(goto_setpoint.max_vertical_speed, 0.f, vehicle_max_vertical_speed);
// linearly scale vertical acceleration limit with vertical speed limit to maintain smoothing dynamic // linearly scale vertical acceleration limit with vertical speed limit to maintain smoothing dynamic
// only limit acceleration once within velocity constraints // only limit acceleration once within velocity constraints
if (fabsf(_position_smoothing.getCurrentVelocityZ()) <= max_vertical_speed) { if (fabsf(_position_smoothing.getCurrentVelocityZ()) <= max_vertical_speed) {
const float speed_scale = max_vertical_speed / vehicle_max_vertical_speed; const float speed_scale = max_vertical_speed / vehicle_max_vertical_speed;
max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, 0.f, max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, 0.f, vehicle_max_vertical_accel);
vehicle_max_vertical_accel);
} }
} }
_position_smoothing.setMaxVelocityZ(max_vertical_speed); _position_smoothing.setMaxVelocityZ(max_vertical_speed);
_position_smoothing.setMaxAccelerationZ(max_vertical_accel); _position_smoothing.setMaxAccelerationZ(max_vertical_accel);
_position_smoothing.setMaxJerkXY(_param_mpc_jerk_auto.get());
_position_smoothing.setMaxJerkZ(_param_mpc_jerk_auto.get()); _position_smoothing.setMaxJerkZ(_param_mpc_jerk_auto.get());
} }

15
src/modules/mc_pos_control/GotoControl/GotoControl.hpp
View File

@ -124,15 +124,16 @@ private:
bool _controlling_heading{false}; bool _controlling_heading{false};
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise, (ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
(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_DOWN_MAX>) _param_mpc_acc_down_max,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_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_JERK_AUTO>) _param_mpc_jerk_auto,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max, (ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
(ParamFloat<px4::params::MPC_YAWRAUTO_ACC>) _param_mpc_yawrauto_acc, (ParamFloat<px4::params::MPC_YAWRAUTO_ACC>) _param_mpc_yawrauto_acc,
(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max (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
); );
}; };