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

Trajectory - Add time synchronization between trajectories. Split update(...) function into updateDurations(...) and integrate(...) to be able to insert time synchronization in between.

Browse Source
This commit is contained in:
bresch
2018-10-03 14:22:31 +02:00
committed by Roman Bapst
parent 86463e4ec7
commit 22780efcd0
3 changed files with 156 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -60,6 +60,8 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
_smoothing[1].setMaxAccel(MPC_ACC_HOR_MAX.get());
_smoothing[0].setMaxVel(_constraints.speed_xy);
_smoothing[1].setMaxVel(_constraints.speed_xy);
_smoothing[0].setDt(_deltatime);
_smoothing[1].setDt(_deltatime);
Vector2f vel_xy = Vector2f(&_velocity(0));
float jerk = _jerk_max.get();
@@ -76,10 +78,15 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
for (int i = 0; i < 2; ++i) {
_smoothing[i].setMaxJerk(jerk);
_smoothing[i].updateDurations(_velocity_setpoint(i));
}
VelocitySmoothing::timeSynchronization(_smoothing, 2);
for (int i = 0; i < 2; ++i) {
float smoothed_velocity_setpoint, smoothed_position_setpoint;
_smoothing[i].update(_deltatime, _position(i), _velocity_setpoint(i),
smoothed_velocity_setpoint, smoothed_position_setpoint);
_smoothing[i].integrate(_position(i), smoothed_velocity_setpoint, smoothed_position_setpoint);
_position_setpoint(i) = smoothed_position_setpoint;
_velocity_setpoint(i) = smoothed_velocity_setpoint;
}