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

Traj Smoothing - make format

Browse Source
This commit is contained in:
bresch
2019-02-08 16:08:14 +01:00
parent 3543071eb9
commit 70ddb8049a
2 changed files with 4 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+3
View File
@@ -155,11 +155,13 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
_smoothing[0].setCurrentPosition(_position(0));
_smoothing[1].setCurrentPosition(_position(1));
}
if (!_position_lock_z_active) {
_smoothing[2].setCurrentPosition(_position(2));
}
Vector3f pos_sp_smooth;
for (int i = 0; i < 3; ++i) {
_smoothing[i].integrate(_acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i));
@@ -175,6 +177,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
_position_setpoint_xy_locked(1) = pos_sp_smooth(1);
_position_lock_xy_active = true;
}
if (fabsf(_vel_sp_smooth(2)) < 0.01f &&
fabsf(_acceleration_setpoint(2)) < .2f &&
fabsf(_sticks_expo(2)) <= FLT_EPSILON) {