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

FlightTasks: use .xy() where possible

Browse Source 
to take the first two elements of a Vector3f.
This commit is contained in:
Matthias Grob
2021-02-10 15:56:58 +01:00
parent 7686533abb
commit 82d6cc3dba
5 changed files with 10 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+4 -4
View File
@@ -69,7 +69,7 @@ void FlightTaskManualPositionSmoothVel::reActivate()
FlightTaskManualPosition::reActivate();
// The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly
// using the generated jerk, reset the z derivatives to zero
_smoothing_xy.reset(Vector2f(), Vector2f(_velocity), Vector2f(_position));
_smoothing_xy.reset(Vector2f(), _velocity.xy(), _position.xy());
_smoothing_z.reset(0.f, 0.f, _position(2));
}
@@ -136,19 +136,19 @@ void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ()
void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback()
{
_smoothing_xy.setVelSpFeedback(Vector2f(_velocity_setpoint_feedback));
_smoothing_xy.setVelSpFeedback(_velocity_setpoint_feedback.xy());
_smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2));
}
void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate()
{
_smoothing_xy.setCurrentPositionEstimate(Vector2f(_position));
_smoothing_xy.setCurrentPositionEstimate(_position.xy());
_smoothing_z.setCurrentPositionEstimate(_position(2));
}
void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target)
{
_smoothing_xy.update(_deltatime, Vector2f(vel_target));
_smoothing_xy.update(_deltatime, vel_target.xy());
_smoothing_z.update(_deltatime, vel_target(2));
}