From 84665672ad11bc9ea7818c1cfc620c59686745de Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 5 Oct 2018 10:51:49 +0200 Subject: [PATCH] Vel smooth - Change jerk scheduling strategy --- .../FlightTaskManualPositionSmoothVel.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 5f21fe4e9f..e18d28a9bc 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -70,7 +70,8 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _smoothing[2].setMaxVel(_constraints.speed_down); } - Vector2f vel_xy = Vector2f(&_velocity(0)); + Vector2f vel_xy_sp = Vector2f(&_velocity_setpoint(0)); + Vector2f vel_xy_sp_smooth = Vector2f(&_vel_sp_smooth(0)); float jerk[3] = {_jerk_max.get(), _jerk_max.get(), _jerk_max.get()}; float jerk_xy = _jerk_max.get(); @@ -79,9 +80,15 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() } if (_jerk_min.get() > FLT_EPSILON) { - // interpolate between min and max jerk - jerk_xy = math::min(_jerk_min.get() + (_jerk_max.get() - _jerk_min.get()) * vel_xy.length() / _constraints.speed_xy, - _jerk_max.get()); + if (vel_xy_sp.length() < FLT_EPSILON) { // Brake + jerk_xy = _jerk_min.get() + (_jerk_max.get() - _jerk_min.get()); + + } else if (vel_xy_sp.dot(vel_xy_sp_smooth) < -FLT_EPSILON) { // Reverse + jerk_xy = _jerk_max.get(); + + } else { + jerk_xy = _jerk_min.get(); + } } jerk[0] = jerk_xy;