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

Trajectory - Add position lock-unlock logic and proper initialization from controller feedback

Browse Source
This commit is contained in:
bresch
2018-10-10 15:59:09 +02:00
committed by Roman Bapst
parent 84665672ad
commit 6a7ce651bc
7 changed files with 59 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+42 -8
View File
@@ -47,6 +47,10 @@ bool FlightTaskManualPositionSmoothVel::activate()
_smoothing[i].reset(0.f, _velocity(i), _position(i));
}
_position_lock_xy_active = false;
_position_setpoint_xy_locked(0) = NAN;
_position_setpoint_xy_locked(1) = NAN;
return ret;
}
@@ -81,9 +85,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
if (_jerk_min.get() > FLT_EPSILON) {
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 {
@@ -94,6 +95,23 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
jerk[0] = jerk_xy;
jerk[1] = jerk_xy;
/* Check for position unlock
* During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
* is continuous. We know that the output of the position loop (part of the velocity setpoint) will suddenly become null
* and only the feedforward (generated by this flight task) will remain. This is why the previous input of the velocity controller
* is used to set current velocity of the trajectory.
*/
Vector2f sticks_expo_xy = Vector2f(&_sticks_expo(0));
if (sticks_expo_xy.length() > FLT_EPSILON) {
if (_position_lock_xy_active) {
_smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(0)); // Start the trajectory at the current velocity setpoint
_smoothing[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
_position_setpoint_xy_locked(0) = NAN;
_position_setpoint_xy_locked(1) = NAN;
}
_position_lock_xy_active = false;
}
for (int i = 0; i < 3; ++i) {
_smoothing[i].setMaxJerk(jerk[i]);
_smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i));
@@ -101,11 +119,27 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only
Vector3f pos_sp_smooth;
Vector3f accel_sp_smooth;
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) = _vel_sp_smooth(i);
_smoothing[i].setCurrentPosition(_position(i));
_smoothing[i].integrate(accel_sp_smooth(i), _vel_sp_smooth(i), pos_sp_smooth(i));
_velocity_setpoint(i) = _vel_sp_smooth(i); // Feedforward
}
// Check for position lock transition
Vector2f accel_setpoint_xy_smooth = Vector2f(&accel_sp_smooth(0));
if (vel_xy_sp_smooth.length() < 0.002f &&
accel_setpoint_xy_smooth.length() < .2f &&
sticks_expo_xy.length() <= FLT_EPSILON) {
if (!_position_lock_xy_active) {
_position_setpoint_xy_locked(0) = pos_sp_smooth(0);
_position_setpoint_xy_locked(1) = pos_sp_smooth(1);
}
_position_lock_xy_active = true;
}
_position_setpoint(0) = _position_setpoint_xy_locked(0);
_position_setpoint(1) = _position_setpoint_xy_locked(1);
}