From c3a4fff0cd6081417be6076929909b69676896e3 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Fri, 26 Oct 2018 11:24:10 +0200
Subject: [PATCH] Auto traj - generate heading from trajectory velocity vector
if possible
---
.../FlightTaskAutoLineSmoothVel.cpp | 33 ++++++++++++++-----
.../FlightTaskAutoLineSmoothVel.hpp | 4 ++-
2 files changed, 28 insertions(+), 9 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
index 7099aff626..6fd8245e33 100644
--- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
@@ -49,6 +49,7 @@ bool FlightTaskAutoLineSmoothVel::activate()
_trajectory[i].reset(0.f, _velocity(i), _position(i));
}
+ _yaw_sp_prev = _yaw;
_updateTrajConstraints();
return ret;
@@ -71,21 +72,37 @@ void FlightTaskAutoLineSmoothVel::_setDefaultConstraints()
void FlightTaskAutoLineSmoothVel::_generateSetpoints()
{
+ _prepareSetpoints();
+ _generateTrajectory();
+
if (!PX4_ISFINITE(_yaw_setpoint)) {
- // no valid heading -> set heading along track
- // TODO: Generate heading along trajectory velocity vector
- _generateHeadingAlongTrack();
+ // no valid heading -> generate heading in this flight task
+ _generateHeading();
}
+}
- _prepareSetpoints();
- _generateTrajectory();
+void FlightTaskAutoLineSmoothVel::_generateHeading()
+{
+ // Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint
+ if (!_generateHeadingAlongTraj()) {
+ _yaw_setpoint = _yaw_sp_prev;
+ }
+
+ _yaw_sp_prev = _yaw_setpoint;
}
-void FlightTaskAutoLineSmoothVel::_generateHeadingAlongTrack()
+bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj()
{
- Vector2f prev_to_dest(_target - _prev_wp);
- _compute_heading_from_2D_vector(_yaw_setpoint, prev_to_dest);
+ bool res = false;
+ Vector2f vel_sp_xy(_velocity_setpoint);
+
+ if (vel_sp_xy.length() > .01f) {
+ // Generate heading from velocity vector, only if it is long enough
+ _compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy);
+ res = true;
+ }
+ return res;
}
/* Constrain some value vith a constrain depending on the sign of the constrain
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
index aaa4e1f05d..d5cd55c836 100644
--- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
@@ -69,9 +69,11 @@ protected:
void _setDefaultConstraints() override;
inline float constrain_one_side(float val, float constrain);
- void _generateHeadingAlongTrack(); /**< Generates heading along track. */
+ void _generateHeading();
+ bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
void _updateTrajConstraints();
void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
void _generateTrajectory();
VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
+ float _yaw_sp_prev;
};
--
GitLab