AutoSmoothVel - scale down acc_hor using traj_p parameter in the comp…

…utation of the maximum waypoint entrance speed
PX4 · Jul 19, 2019 · 06a0bf9 · 06a0bf9
1 parent 8f63609
commit 06a0bf9
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 10 deletions.
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
@@ -196,14 +196,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;

diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -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
  */