fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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 12:03:42 +01:00
parent 7686533abb
commit 82d6cc3dba
5 changed files with 10 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp
View File

@ -51,7 +51,7 @@ bool FlightTaskManualAcceleration::activate(const vehicle_local_position_setpoin
_velocity_setpoint.xy() = Vector2f(last_setpoint.vx, last_setpoint.vy);
} else {
_velocity_setpoint.xy() = Vector2f(_velocity);
_velocity_setpoint.xy() = _velocity.xy();
}
_stick_acceleration_xy.resetPosition();
@ -70,7 +70,7 @@ bool FlightTaskManualAcceleration::update()
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint,
_sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, _position,
Vector2f(_velocity_setpoint_feedback), _deltatime);
_velocity_setpoint_feedback.xy(), _deltatime);
_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);
_constraints.want_takeoff = _checkTakeoff();
@ -84,5 +84,5 @@ void FlightTaskManualAcceleration::_ekfResetHandlerPositionXY()
void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY()
{
_stick_acceleration_xy.resetVelocity(Vector2f(_velocity));
_stick_acceleration_xy.resetVelocity(_velocity.xy());
}

3
src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
View File

@ -112,8 +112,7 @@ void FlightTaskManualPosition::_scaleSticks()
// collision prevention
if (_collision_prevention.is_active()) {
_collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, Vector2f(_position),
Vector2f(_velocity));
_collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, _position.xy(), _velocity.xy());
}
_velocity_setpoint.xy() = vel_sp_xy;

8
src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
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));
}

2
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -159,7 +159,7 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set
bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint);
_r = _radius_min;
_v = 1.f;
_center = Vector2f(_position);
_center = _position.xy();
_center(0) -= _r;
_initial_heading = _yaw;
_slew_rate_yaw.setForcedValue(_yaw);

2
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
View File

@ -151,7 +151,7 @@ void StickAccelerationXY::lockPosition(const Vector3f &pos, const matrix::Vector
{
if (_velocity_setpoint.norm_squared() < FLT_EPSILON) {
if (!PX4_ISFINITE(_position_setpoint(0))) {
_position_setpoint = Vector2f(pos);
_position_setpoint = pos.xy();
}
} else {