fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-12 20:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Vel smooth - Add Z trajectory

Browse Source
This commit is contained in:
bresch
2018-10-05 10:36:59 +02:00
committed by Roman Bapst
parent 787d82c9e6
commit ab7e4436b3
2 changed files with 27 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+23 -12
View File
@@ -61,8 +61,18 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
_smoothing[0].setMaxVel(_constraints.speed_xy);
_smoothing[1].setMaxVel(_constraints.speed_xy);
if (_velocity_setpoint(2) < 0.f) { // up
_smoothing[2].setMaxAccel(MPC_ACC_UP_MAX.get());
_smoothing[2].setMaxVel(_constraints.speed_up);
} else { // down
_smoothing[2].setMaxAccel(MPC_ACC_DOWN_MAX.get());
_smoothing[2].setMaxVel(_constraints.speed_down);
}
Vector2f vel_xy = Vector2f(&_velocity(0));
float jerk = _jerk_max.get();
float jerk[3] = {_jerk_max.get(), _jerk_max.get(), _jerk_max.get()};
float jerk_xy = _jerk_max.get();
if (_jerk_min.get() > _jerk_max.get()) {
_jerk_min.set(0.f);
@@ -70,24 +80,25 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
if (_jerk_min.get() > FLT_EPSILON) {
// interpolate between min and max jerk
jerk = math::min(_jerk_min.get() + (_jerk_max.get() - _jerk_min.get()) * vel_xy.length() / _constraints.speed_xy,
jerk_xy = math::min(_jerk_min.get() + (_jerk_max.get() - _jerk_min.get()) * vel_xy.length() / _constraints.speed_xy,
_jerk_max.get());
}
for (int i = 0; i < 2; ++i) {
_smoothing[i].setMaxJerk(jerk);
jerk[0] = jerk_xy;
jerk[1] = jerk_xy;
for (int i = 0; i < 3; ++i) {
_smoothing[i].setMaxJerk(jerk[i]);
_smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i));
}
VelocitySmoothing::timeSynchronization(_smoothing, 2);
VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only
for (int i = 0; i < 2; ++i) {
float smoothed_velocity_setpoint, smoothed_position_setpoint;
_smoothing[i].integrate(_position(i), smoothed_velocity_setpoint, smoothed_position_setpoint);
for (int i = 0; i < 3; ++i) {
float smoothed_position_setpoint;
_smoothing[i].integrate(_position(i), _vel_sp_smooth(i), smoothed_position_setpoint);
//printf("\nTraj %d\tNew total time = %.3f", i, (double)_smoothing[i].getTotalTime());
_position_setpoint(i) = smoothed_position_setpoint;
_velocity_setpoint(i) = smoothed_velocity_setpoint;
_velocity_setpoint(i) = _vel_sp_smooth(i);
}
// TODO: z
}