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

FlightTaskAutoLine: don't reset during normal operation

Browse Source
This commit is contained in:
Dennis Mannhart
2018-07-20 09:26:18 +02:00
committed by Lorenz Meier
parent 26ca17e329
commit 65603d7de7
1 changed files with 7 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp
+7 -11
View File
@@ -123,10 +123,9 @@ void FlightTaskAutoLine::_reset()
void FlightTaskAutoLine::_generateIdleSetpoints() void FlightTaskAutoLine::_generateIdleSetpoints()
{ {
/* Send zero thrust setpoint */ /* Send zero thrust setpoint */
_position_setpoint = Vector3f(NAN, NAN, NAN); // Don't require any position/velocity setpoints
_velocity_setpoint = Vector3f(NAN, NAN, NAN);
_thrust_setpoint.zero(); _thrust_setpoint.zero();
/* Set member setpoints to current state */
_reset();
} }
void FlightTaskAutoLine::_generateLandSetpoints() void FlightTaskAutoLine::_generateLandSetpoints()
@@ -134,15 +133,14 @@ void FlightTaskAutoLine::_generateLandSetpoints()
/* Keep xy-position and go down with landspeed. */ /* Keep xy-position and go down with landspeed. */
_position_setpoint = Vector3f(_target(0), _target(1), NAN); _position_setpoint = Vector3f(_target(0), _target(1), NAN);
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _land_speed.get())); _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _land_speed.get()));
/* Set member setpoints to current state */
_reset();
} }
void FlightTaskAutoLine::_generateTakeoffSetpoints() void FlightTaskAutoLine::_generateTakeoffSetpoints()
{ {
/* Takeoff is completely defined by target position. */ /* Takeoff is completely defined by target position. */
_position_setpoint = _target; _position_setpoint = _target;
_velocity_setpoint = Vector3f(NAN, NAN, NAN);
}
void FlightTaskAutoLine::_generateVelocitySetpoints() void FlightTaskAutoLine::_generateVelocitySetpoints()
{ {
@@ -151,8 +149,6 @@ void FlightTaskAutoLine::_generateVelocitySetpoints()
_position_setpoint = Vector3f(NAN, NAN, _position(2)); _position_setpoint = Vector3f(NAN, NAN, _position(2));
Vector2f vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed; Vector2f vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed;
_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN); _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
/* Set member setpoints to current state */
_reset();
} }
void FlightTaskAutoLine::_generateSetpoints() void FlightTaskAutoLine::_generateSetpoints()
@@ -165,8 +161,8 @@ void FlightTaskAutoLine::_generateSetpoints()
void FlightTaskAutoLine::_updateInternalWaypoints() void FlightTaskAutoLine::_updateInternalWaypoints()
{ {
/* The internal Waypoints might differ from previous_wp and target. The cases where it differs: /* The internal Waypoints might differ from previous_wp and target. The cases where it differs:
* 1. The vehicle already passe the target -> go straight to target * 1. The vehicle already passed the target -> go straight to target
* 2. The vehicle is more than cruise speed in front of previous waypoint -> go straight to previous wp * 2. The vehicle is more than cruise speed in front of previous waypoint -> go straight to previous waypoint
* 3. The vehicle is more than cruise speed from track -> go straight to closest point on track * 3. The vehicle is more than cruise speed from track -> go straight to closest point on track
* *
* If a new target is available, then the speed at the target is computed from the angle previous-target-next * If a new target is available, then the speed at the target is computed from the angle previous-target-next
@@ -288,7 +284,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
/* Vehicle reached target in xy and no passing required. Lock position */ /* Vehicle reached target in xy and no passing required. Lock position */
_position_setpoint(0) = _destination(0); _position_setpoint(0) = _destination(0);
_position_setpoint(1) = _destination(1); _position_setpoint(1) = _destination(1);
_velocity_setpoint.zero(); _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
} else { } else {