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

FlightTaskAutoLine: simplify logic by setting setpoints directly

Browse Source
This commit is contained in:
Dennis Mannhart
2018-07-20 09:26:18 +02:00
committed by Lorenz Meier
parent 338130a9b4
commit f62c3c3b8a
2 changed files with 22 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp
+21 -19
View File
@@ -81,9 +81,7 @@ bool FlightTaskAutoLine::update()
_generateVelocitySetpoints(); _generateVelocitySetpoints();
} }
/* Send setpoints */ /* For now yaw setpoint comes directly form triplets */
_setVelocitySetpoint(Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), _vel_sp_z));
_setPositionSetpoint(Vector3f(_pos_sp_xy(0), _pos_sp_xy(1), _pos_sp_z));
_setYawSetpoint(_yaw_wp); _setYawSetpoint(_yaw_wp);
return true; return true;
@@ -104,29 +102,29 @@ void FlightTaskAutoLine::_reset()
void FlightTaskAutoLine::_generateIdleSetpoints() void FlightTaskAutoLine::_generateIdleSetpoints()
{ {
/* Send zero thrust setpoint */ /* Send zero thrust setpoint */
_vel_sp_xy = matrix::Vector2f(NAN, NAN);
_vel_sp_z = NAN;
_pos_sp_xy = matrix::Vector2f(NAN, NAN);
_pos_sp_z = NAN;
_setThrustSetpoint(Vector3f(0.0f, 0.0f, 0.0f)); _setThrustSetpoint(Vector3f(0.0f, 0.0f, 0.0f));
/* Set member setpoints to current state */
_reset();
} }
void FlightTaskAutoLine::_generateLandSetpoints() void FlightTaskAutoLine::_generateLandSetpoints()
{ {
/* Keep xy-position and go down with landspeed. */ /* Keep xy-position and go down with landspeed. */
_pos_sp_xy = Vector2f(&_target(0)); _setPositionSetpoint(Vector3f(_target(0), _target(1), NAN));
_vel_sp_z = _land_speed.get(); _setVelocitySetpoint(Vector3f(NAN, NAN, _land_speed.get()));
_pos_sp_z = NAN;
_vel_sp_xy = Vector2f(NAN, NAN); /* Set member setpoints to current state */
_reset();
} }
void FlightTaskAutoLine::_generateTakeoffSetpoints() void FlightTaskAutoLine::_generateTakeoffSetpoints()
{ {
/* Takeoff is completely defined by position. */ /* Takeoff is completely defined by position. */
_pos_sp_xy = Vector2f(&_target(0)); _setPositionSetpoint(_target);
_pos_sp_z = _target(2);
_vel_sp_xy.zero(); /* Set member setpoints to current state */
_vel_sp_z = 0.0f; _reset();
} }
void FlightTaskAutoLine::_generateSetpoints() void FlightTaskAutoLine::_generateSetpoints()
@@ -134,6 +132,8 @@ void FlightTaskAutoLine::_generateSetpoints()
_updateInternalWaypoints(); _updateInternalWaypoints();
_generateAltitudeSetpoints(); _generateAltitudeSetpoints();
_generateXYsetpoints(); _generateXYsetpoints();
_setPositionSetpoint(Vector3f(_pos_sp_xy(0), _pos_sp_xy(1), _pos_sp_z));
_setVelocitySetpoint(Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), _vel_sp_z));
} }
void FlightTaskAutoLine::_updateInternalWaypoints() void FlightTaskAutoLine::_updateInternalWaypoints()
@@ -339,6 +339,7 @@ void FlightTaskAutoLine::_generateXYsetpoints()
if (PX4_ISFINITE(_yaw_wp)) { if (PX4_ISFINITE(_yaw_wp)) {
yaw_diff = _wrap_pi(_yaw_wp - _yaw); yaw_diff = _wrap_pi(_yaw_wp - _yaw);
PX4_WARN("Yaw Waypoint not finite");
} }
/* If yaw offset is large, only accelerate with 0.5 m/s^2. */ /* If yaw offset is large, only accelerate with 0.5 m/s^2. */
@@ -433,10 +434,11 @@ void FlightTaskAutoLine::_generateVelocitySetpoints()
{ {
/* TODO: Remove velocity force logic from navigator, since /* TODO: Remove velocity force logic from navigator, since
* navigator should only send out waypoints. */ * navigator should only send out waypoints. */
_vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed; _setPositionSetpoint(Vector3f(NAN, NAN, _position(2)));
_vel_sp_z = 0.0f; Vector2f vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed;
_pos_sp_z = _position(2); _setVelocitySetpoint(Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN));
_pos_sp_xy = Vector2f(NAN, NAN);
_reset();
} }
float FlightTaskAutoLine::_getVelocityFromAngle(const float angle) float FlightTaskAutoLine::_getVelocityFromAngle(const float angle)
src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp
+1 -3
View File
@@ -54,9 +54,6 @@ public:
bool update() override; bool update() override;
protected: protected:
void _reset() override;
matrix::Vector2f _vel_sp_xy{}; matrix::Vector2f _vel_sp_xy{};
matrix::Vector2f _pos_sp_xy{}; matrix::Vector2f _pos_sp_xy{};
float _vel_sp_z = 0.0f; float _vel_sp_z = 0.0f;
@@ -98,4 +95,5 @@ protected:
private: private:
float _getVelocityFromAngle(const float angle); /** Computes the speed at target depending on angle. */ float _getVelocityFromAngle(const float angle); /** Computes the speed at target depending on angle. */
void _reset(); /** Resets member variables to current vehicle state */
}; };