From 72a213aff71af09b21db0b14295107a99843837d Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Thu, 24 May 2018 08:51:24 +0200
Subject: [PATCH] FlightTaskManualAltitude: valid altitude/climb-rate is
required
---
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp | 10 +++++++---
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp | 1 +
2 files changed, 8 insertions(+), 3 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
index f5e31e737e..43579103b8 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
@@ -40,6 +40,13 @@
using namespace matrix;
+bool FlightTaskManualAltitude::updateInitialize()
+{
+ bool ret = FlightTaskManualStabilized::updateInitialize();
+ // in addition to stabilized require valid position and velocity in D-direction
+ return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2));
+}
+
bool FlightTaskManualAltitude::activate()
{
bool ret = FlightTaskManualStabilized::activate();
@@ -47,9 +54,6 @@ 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/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
index ef45a1b0d6..b5c2b31747 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
@@ -47,6 +47,7 @@ public:
FlightTaskManualAltitude() = default;
virtual ~FlightTaskManualAltitude() = default;
bool activate() override;
+ bool updateInitialize() override;
protected:
void _updateSetpoints() override; /**< updates all setpoints */
--
GitLab