From bb055fdaf3d1d846538d296fa664eecca2f68798 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 14 May 2019 11:07:34 +0200 Subject: [PATCH] AutoSmoothVel - Override checkTakeoff with task-specific logic and reactivate z axis with downward velocity to takeoff smoothly --- .../FlightTaskAutoLineSmoothVel.cpp | 12 ++++++++---- .../FlightTaskAutoLineSmoothVel.hpp | 7 ++++++- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index d6ed9994ec..f03e1fab33 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -57,10 +57,12 @@ bool FlightTaskAutoLineSmoothVel::activate() void FlightTaskAutoLineSmoothVel::reActivate() { - // Don't reset during takeoff TODO: Find a proper solution - // The issue here is that with a small increment of velocity setpoint (generated by this flight task), the - // land detector doesn't detect takeoff and without takeoff detection, the - // flight task is always reset. + // On ground, reset acceleration and velocity to zero + for (int i = 0; i < 2; ++i) { + _trajectory[i].reset(0.f, 0.f, _position(i)); + } + + _trajectory[2].reset(0.f, 0.7f, _position(2)); } void FlightTaskAutoLineSmoothVel::_generateSetpoints() @@ -143,6 +145,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() // If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here. _checkEkfResetCounters(); + _want_takeoff = false; if (PX4_ISFINITE(_position_setpoint(0)) && PX4_ISFINITE(_position_setpoint(1))) { @@ -191,6 +194,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() _velocity_setpoint(2) = vel_sp_z; } + _want_takeoff = _velocity_setpoint(2) < -0.3f; } } diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index e3fd4055ce..98c8517146 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -67,6 +67,9 @@ protected: void _generateSetpoints() override; /**< Generate setpoints along line. */ + /** determines when to trigger a takeoff (ignored in flight) */ + bool _checkTakeoff() override { return _want_takeoff; }; + inline float _constrainOneSide(float val, float constrain); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ void _generateHeading(); @@ -75,7 +78,9 @@ protected: 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; + + float _yaw_sp_prev{NAN}; + bool _want_takeoff{false}; /* counters for estimator local position resets */ struct {