diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp index d41d347445..9dd2569cbf 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp @@ -41,6 +41,16 @@ using namespace matrix; +bool FlightTaskManualPosition::updateInitialize() +{ + bool ret = FlightTaskManualAltitude::updateInitialize(); + // require valid position / velocity in xy + return ret && PX4_ISFINITE(_position(0)) + && PX4_ISFINITE(_position(1)) + && PX4_ISFINITE(_velocity(0)) + && PX4_ISFINITE(_velocity(1)); +} + bool FlightTaskManualPosition::activate() { @@ -59,10 +69,6 @@ bool FlightTaskManualPosition::activate() // 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/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp index ff789eaa04..8ebaa33681 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp @@ -49,6 +49,7 @@ public: virtual ~FlightTaskManualPosition() = default; bool activate() override; + bool updateInitialize() override; protected: void _updateXYlock(); /**< applies position lock based on stick and velocity */