From 216279b5761ad1a6738c671d6c4152daf5f71582 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 2 May 2018 11:47:56 +0200 Subject: [PATCH] FlightTask: activate tasks with setpoints in use --- src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp | 1 + src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp | 3 +++ src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp | 4 +++- src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp | 2 ++ src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp | 2 ++ 5 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index 1c83b52ad2..000c73b183 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -100,6 +100,7 @@ void FlightTaskAutoLine::_reset() // Set setpoints equal current state. _velocity_setpoint = _velocity; _position_setpoint = _position; + _yaw_setpoint = _yaw; _destination = _target; _origin = _prev_wp; _speed_at_target = 0.0f; diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index b4e2cc44d3..72761305a0 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -43,6 +43,9 @@ using namespace matrix; bool FlightTaskManualAltitude::activate() { bool ret = FlightTaskManualStabilized::activate(); + _thrust_setpoint(2) = NAN; // altitude is controlled from position/velocity + _position_setpoint(2) = _position(2); + _velocity_setpoint(2) = 0.0f; _setDefaultConstraints(); return ret; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp index 43b3e81087..54298f37c8 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp @@ -51,8 +51,10 @@ bool FlightTaskManualPosition::activate() _constraints.speed_xy = MPC_VEL_MANUAL.get(); } + _position_setpoint(0) = _position(0); + _position_setpoint(1) = _position(1); + _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f; _velocity_scale = _constraints.speed_xy; - return ret; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp index 3b73976747..79c90345dc 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp @@ -58,6 +58,8 @@ bool FlightTaskManualStabilized::activate() { bool ret = FlightTaskManual::activate(); _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get()); + _yaw_setpoint = _yaw; + _yawspeed_setpoint = 0.0f; _constraints.tilt = math::radians(_tilt_max_man.get()); return ret; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp b/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp index 85889cde18..88295c001d 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp @@ -55,6 +55,8 @@ bool FlightTaskOffboard::initializeSubscriptions(SubscriptionArray &subscription bool FlightTaskOffboard::activate() { bool ret = FlightTask::activate(); + _position_setpoint = _position; + _velocity_setpoint *= 0.0f; _position_lock *= NAN; return ret; }