fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 22:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManualPositionSmooth: smooth xy and z

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent ac6ed74063
commit ed62056b7c
4 changed files with 42 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp
+5 -2
View File
@@ -116,8 +116,9 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp)
/* Distinguish between acceleration, deceleration and direction change */
/* Check if stick direction and current velocity are within 120.
* If previous and current setpoint are more than 120 apart, we assume
* that the user demanded a direction change. */
* If current setpoint and velocity are more than 120 apart, we assume
* that the user demanded a direction change.
* The detecton has to happen in body frame.*/
matrix::Vector2f vel_sp_unit = vel_sp;;
matrix::Vector2f vel_sp_prev_unit = _vel_sp_prev;
@@ -238,4 +239,6 @@ ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt)
if (acc.length() > _acc_state_dependent) {
vel_sp = acc.normalized() * _acc_state_dependent * dt + _vel_sp_prev;
}
_vel_sp_prev = vel_sp;
}