Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

ManualPositionSmoothVel - Force velocity and acceleration setpoints to zero if the velocity setpoint is smaller than 1mm/s and that the acceleration setpoint is null

Browse Source
This commit is contained in:
bresch
2019-03-07 14:03:11 +01:00
committed by Beat Küng
parent 1fefa76e5d
commit aecf8fce58
1 changed files with 21 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+21 -3
View File
@@ -183,20 +183,19 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
Vector3f pos_sp_smooth; Vector3f pos_sp_smooth;
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
_smoothing[i].integrate(_acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i)); _smoothing[i].integrate(_acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i));
_velocity_setpoint(i) = _vel_sp_smooth(i); // Feedforward _velocity_setpoint(i) = _vel_sp_smooth(i); // Feedforward
_jerk_setpoint(i) = _smoothing[i].getCurrentJerk(); _jerk_setpoint(i) = _smoothing[i].getCurrentJerk();
} }
// Check for position lock transition // Check for position lock transition
if (Vector2f(_vel_sp_smooth).length() < 0.01f && if (Vector2f(_vel_sp_smooth).length() < 0.1f &&
Vector2f(_acceleration_setpoint).length() < .2f && Vector2f(_acceleration_setpoint).length() < .2f &&
sticks_expo_xy.length() <= FLT_EPSILON) { sticks_expo_xy.length() <= FLT_EPSILON) {
_position_lock_xy_active = true; _position_lock_xy_active = true;
} }
if (fabsf(_vel_sp_smooth(2)) < 0.01f && if (fabsf(_vel_sp_smooth(2)) < 0.1f &&
fabsf(_acceleration_setpoint(2)) < .2f && fabsf(_acceleration_setpoint(2)) < .2f &&
fabsf(_sticks_expo(2)) <= FLT_EPSILON) { fabsf(_sticks_expo(2)) <= FLT_EPSILON) {
_position_lock_z_active = true; _position_lock_z_active = true;
@@ -209,10 +208,29 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
if (_position_lock_xy_active) { if (_position_lock_xy_active) {
_position_setpoint_xy_locked(0) = pos_sp_smooth(0); _position_setpoint_xy_locked(0) = pos_sp_smooth(0);
_position_setpoint_xy_locked(1) = pos_sp_smooth(1); _position_setpoint_xy_locked(1) = pos_sp_smooth(1);
// If the velocity setpoint is smaller than 1mm/s and that the acceleration is 0, force the setpoints
// to zero. This is required because the generated velocity is never exactly zero and if the drone hovers
// for a long period of time, thr drift of the position setpoint will be noticeable.
for (int i = 0; i < 2; i++) {
if (fabsf(_velocity_setpoint(i)) < 1e-3f && fabsf(_acceleration_setpoint(0)) < FLT_EPSILON) {
_velocity_setpoint(i) = 0.f;
_acceleration_setpoint(i) = 0.f;
_smoothing[i].setCurrentVelocity(0.f);
_smoothing[i].setCurrentAcceleration(0.f);
}
}
} }
if (_position_lock_z_active) { if (_position_lock_z_active) {
_position_setpoint_z_locked = pos_sp_smooth(2); _position_setpoint_z_locked = pos_sp_smooth(2);
if (fabsf(_velocity_setpoint(2)) < 1e-3f && fabsf(_acceleration_setpoint(2)) < FLT_EPSILON) {
_velocity_setpoint(2) = 0.f;
_acceleration_setpoint(2) = 0.f;
_smoothing[2].setCurrentVelocity(0.f);
_smoothing[2].setCurrentAcceleration(0.f);
}
} }
_position_setpoint(0) = _position_setpoint_xy_locked(0); _position_setpoint(0) = _position_setpoint_xy_locked(0);