yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Vel smooth - Change jerk scheduling strategy

Browse Source
This commit is contained in:
bresch
2018-10-05 10:51:49 +02:00
committed by Roman Bapst
parent ab7e4436b3
commit 84665672ad
1 changed files with 11 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
View File

@@ -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;