From afd02aca92564e9e2be05942233f5d31773c797a Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Tue, 22 May 2018 14:23:58 +0200 Subject: [PATCH] FlightTask: fail activation if required states are not available --- src/lib/FlightTasks/tasks/FlightTaskAuto.cpp | 8 ++++++++ src/lib/FlightTasks/tasks/FlightTaskManual.cpp | 15 +++++++++++++++ src/lib/FlightTasks/tasks/FlightTaskManual.hpp | 4 ++-- .../tasks/FlightTaskManualAltitude.cpp | 3 +++ .../tasks/FlightTaskManualPosition.cpp | 8 ++++++++ .../tasks/FlightTaskManualStabilized.cpp | 4 ++++ src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp | 9 +++++++++ src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp | 9 +++++++++ 8 files changed, 58 insertions(+), 2 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp index ac48d4fcfe..b0b056c9b0 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp @@ -63,6 +63,14 @@ bool FlightTaskAuto::activate() _prev_prev_wp = _prev_wp = _target = _next_wp = _position; _setDefaultConstraints(); + // need a valid position and velocity + ret = ret && PX4_ISFINITE(_position(0)) + && PX4_ISFINITE(_position(1)) + && PX4_ISFINITE(_position(2)) + && PX4_ISFINITE(_velocity(0)) + && PX4_ISFINITE(_velocity(1)) + && PX4_ISFINITE(_velocity(2)); + return ret; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index 22039dddd0..eaef2e9b89 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -65,6 +65,21 @@ bool FlightTaskManual::updateInitialize() return ret; } +bool FlightTaskManual::activate() +{ + bool ret = FlightTask::activate(); + + if (_sticks_data_required) { + // need valid stick inputs + ret = ret && PX4_ISFINITE(_sticks(0)) + && PX4_ISFINITE(_sticks(1)) + && PX4_ISFINITE(_sticks(2)) + && PX4_ISFINITE(_sticks(3)); + } + + return ret; +} + bool FlightTaskManual::_evaluateSticks() { /* Sticks are rescaled linearly and exponentially to [-1,1] */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index da48801dfa..ef5fad4f40 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -51,10 +51,10 @@ public: virtual ~FlightTaskManual() = default; bool initializeSubscriptions(SubscriptionArray &subscription_array) override; - bool applyCommandParameters(const vehicle_command_s &command) override { return FlightTask::applyCommandParameters(command); }; - bool updateInitialize() override; + bool activate() override; + protected: diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index e2bc9a18cd..f5e31e737e 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -47,6 +47,9 @@ bool FlightTaskManualAltitude::activate() _position_setpoint(2) = _position(2); _velocity_setpoint(2) = 0.0f; _setDefaultConstraints(); + + // altitude-mode requires to have a valid position and velocity state in D-direction + ret = ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)); return ret; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp index 54298f37c8..d41d347445 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp @@ -44,6 +44,7 @@ using namespace matrix; bool FlightTaskManualPosition::activate() { + // all requirements from altitude-mode still have to hold bool ret = FlightTaskManualAltitude::activate(); // set task specific constraint @@ -55,6 +56,13 @@ bool FlightTaskManualPosition::activate() _position_setpoint(1) = _position(1); _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f; _velocity_scale = _constraints.speed_xy; + + // for position-controlled mode, we need a valid position and velocity state + // in NE-direction + ret = ret && PX4_ISFINITE(_position(0)) + && PX4_ISFINITE(_position(1)) + && PX4_ISFINITE(_velocity(0)) + && PX4_ISFINITE(_velocity(1)); return ret; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp index 8345ec1ce3..29597e2625 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp @@ -62,6 +62,10 @@ bool FlightTaskManualStabilized::activate() _yaw_setpoint = _yaw; _yawspeed_setpoint = 0.0f; _constraints.tilt = math::radians(_tilt_max_man.get()); + + // need a valid yaw + ret = ret && PX4_ISFINITE(_yaw); + return ret; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp b/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp index 88295c001d..043f3979a6 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp @@ -58,6 +58,15 @@ bool FlightTaskOffboard::activate() _position_setpoint = _position; _velocity_setpoint *= 0.0f; _position_lock *= NAN; + + // need a valid position and velocity + ret = ret && PX4_ISFINITE(_position(0)) + && PX4_ISFINITE(_position(1)) + && PX4_ISFINITE(_position(2)) + && PX4_ISFINITE(_velocity(0)) + && PX4_ISFINITE(_velocity(1)) + && PX4_ISFINITE(_velocity(2)); + return ret; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp index 4b2e8f1140..4918e6f11a 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp @@ -67,6 +67,15 @@ bool FlightTaskOrbit::activate() _z = _position(2); _center = Vector2f(_position.data()); _center(0) -= _r; + + // need a valid position and velocity + ret = ret && PX4_ISFINITE(_position(0)) + && PX4_ISFINITE(_position(1)) + && PX4_ISFINITE(_position(2)) + && PX4_ISFINITE(_velocity(0)) + && PX4_ISFINITE(_velocity(1)) + && PX4_ISFINITE(_velocity(2)); + return ret; }