Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

AutoSmoothVel - scale down acc_hor using traj_p parameter in the computation of the maximum waypoint entrance speed

Browse Source
This commit is contained in:
bresch
2019-07-19 14:37:03 +02:00
committed by Mathieu Bresciani
parent de10f1e04d
commit b1698b78bc
2 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+7 -7
View File
@@ -199,14 +199,14 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
yaw_align_check_pass) {
// Max speed between current and next
const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next);
const float alpha = acos(Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() * Vector2f(&(_target - _next_wp)(
0)).unit_or_zero()) / 2.f;
const float alpha = acos(Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() *
Vector2f(&(_target - _next_wp)(0)).unit_or_zero()) / 2.f;
const float tan_alpha = tan(alpha);
// We choose a maximum centripetal acceleration of MPC_ACC_HOR/2 to take in account that there is a jerk limit (a direct transition from line to circle is not possible)
// We assume that the real radius of the acceptance radius is half of the _target_acceptance_radius since Navigator switches for us depending on the current position of
// the drone. This allows for some tolerance on tracking error.
speed_at_target = math::min(math::min(sqrtf(_param_mpc_acc_hor.get() / 2.f * _target_acceptance_radius / 2.f *
tan_alpha), max_speed_current_next), _mc_cruise_speed);
// We choose a maximum centripetal acceleration of MPC_ACC_HOR * MPC_XY_TRAJ_P to take in account that there is a jerk limit (a direct transition from line to circle is not possible)
// MPC_XY_TRAJ_P should be between 0 and 1.
const float max_speed_in_turn = sqrtf(_param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get() * _target_acceptance_radius
* tan_alpha);
speed_at_target = math::min(math::min(max_speed_in_turn, max_speed_current_next), _mc_cruise_speed);
}
return speed_at_target;
src/modules/mc_pos_control/mc_pos_control_params.c
+3 -3
View File
@@ -253,17 +253,17 @@ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
* Proportional gain for horizontal trajectory position error
*
* @min 0.1
* @max 5.0
* @max 1.0
* @decimal 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.3f);
PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f);
/**
* Proportional gain for vertical trajectory position error
*
* @min 0.1
* @max 5.0
* @max 1.0
* @decimal 1
* @group Multicopter Position Control
*/