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

FlightTaskManualPosition: update to new smoothing interface

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent 4f17fb1303
commit bb00f6d737
1 changed files with 4 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp
+4 -12
View File
@@ -45,10 +45,7 @@ FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth(control::SuperBlo
FlightTaskManualPosition(parent, name), FlightTaskManualPosition(parent, name),
_smoothingXY(matrix::Vector2f(&_velocity(0))), _smoothingXY(matrix::Vector2f(&_velocity(0))),
_smoothingZ(_velocity(2), _sticks(2)) _smoothingZ(_velocity(2), _sticks(2))
{}
{
}
bool FlightTaskManualPositionSmooth::activate() bool FlightTaskManualPositionSmooth::activate()
@@ -59,25 +56,20 @@ bool FlightTaskManualPositionSmooth::activate()
void FlightTaskManualPositionSmooth::_updateSetpoints() void FlightTaskManualPositionSmooth::_updateSetpoints()
{ {
/* Get yaw, unsmoothe position setpoints */ /* Get yaw setpont, unsmoothed position setpoints */
FlightTaskManualPosition::_updateSetpoints(); FlightTaskManualPosition::_updateSetpoints();
/* Smooth velocity setpoint in xy */ /* Smooth velocity setpoint in xy */
matrix::Vector2f vel(&_velocity(0)); matrix::Vector2f vel(&_velocity(0));
_smoothingXY.smoothVelFromSticks(_vel_sp_xy, vel, _deltatime); _smoothingXY.smoothVelocity(_vel_sp_xy, vel, _yaw, _yaw_rate_sp, _deltatime);
/* Check for altitude lock*/ /* Check for altitude lock*/
_updateXYlock(); _updateXYlock();
/* Smooth velocity in z*/ /* Smooth velocity in z*/
float vel_sp[2] = { _vel_sp_z, _vel_sp_prev_z }; _smoothingZ.smoothVelFromSticks(_vel_sp_z, _deltatime);
_smoothingZ.smoothVelFromSticks(vel_sp, _deltatime);
_vel_sp_z = vel_sp[0];
/* Check for altitude lock*/ /* Check for altitude lock*/
_updateAltitudeLock(); _updateAltitudeLock();
/* Update previous velocity setpoint for next smoothing iteration */
_vel_sp_prev_z = _vel_sp_z;
} }