From dca378fbfde2e8177955388173f9f8b7ed2587f1 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Fri, 27 Apr 2018 15:10:46 +0200 Subject: [PATCH] FollowMe: legacy implementation. NOTE: FOLLOW-ME is already broken on legacy code. --- src/lib/FlightTasks/CMakeLists.txt | 1 + src/lib/FlightTasks/FlightTasks.cpp | 4 ++ src/lib/FlightTasks/FlightTasks.hpp | 3 ++ src/lib/FlightTasks/tasks/FlightTaskAuto.cpp | 29 ++++++++--- src/lib/FlightTasks/tasks/FlightTaskAuto.hpp | 7 +-- .../tasks/FlightTaskAutoFollowMe.cpp | 49 ++++++++++++++++++ .../tasks/FlightTaskAutoFollowMe.hpp | 50 +++++++++++++++++++ .../FlightTasks/tasks/FlightTaskAutoLine.cpp | 10 +--- 8 files changed, 135 insertions(+), 18 deletions(-) create mode 100644 src/lib/FlightTasks/tasks/FlightTaskAutoFollowMe.cpp create mode 100644 src/lib/FlightTasks/tasks/FlightTaskAutoFollowMe.hpp diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index 3c30449998..66aaf56645 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -42,6 +42,7 @@ tasks/FlightTask.cpp tasks/FlightTaskManualPositionSmooth.cpp tasks/FlightTaskAuto.cpp tasks/FlightTaskAutoLine.cpp + tasks/FlightTaskAutoFollowMe.cpp tasks/Utility/ManualSmoothingZ.cpp tasks/Utility/ManualSmoothingXY.cpp SubscriptionArray.cpp diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index c68bc62bcc..38cec0092e 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/src/lib/FlightTasks/FlightTasks.cpp @@ -86,6 +86,10 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index) _current_task = new (&_task_union.autoLine) FlightTaskAutoLine(); break; + case FlightTaskIndex::AutoFollowMe: + _current_task = new (&_task_union.autoFollowMe) FlightTaskAutoFollowMe(); + break; + default: /* invalid task */ return -1; diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index 05b4a88deb..a2343108cd 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -48,6 +48,7 @@ #include "tasks/FlightTaskManualPositionSmooth.hpp" #include "tasks/FlightTaskManualStabilized.hpp" #include "tasks/FlightTaskAutoLine.hpp" +#include "tasks/FlightTaskAutoFollowMe.hpp" #include "tasks/FlightTaskOrbit.hpp" #include "tasks/FlightTaskSport.hpp" @@ -65,6 +66,7 @@ enum class FlightTaskIndex : int { Orbit, Sport, AutoLine, + AutoFollowMe, Count // number of tasks }; @@ -148,6 +150,7 @@ private: FlightTaskOrbit orbit; FlightTaskSport sport; FlightTaskAutoLine autoLine; + FlightTaskAutoFollowMe autoFollowMe; } _task_union; /**< storage for the currently active task */ FlightTask *_current_task = nullptr; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp index 9adbb0d85b..992881268e 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp @@ -61,7 +61,6 @@ bool FlightTaskAuto::activate() { bool ret = FlightTask::activate(); _prev_prev_wp = _prev_wp = _target = _next_wp = _position; - _yaw_wp = _yaw; _setDefaultConstraints(); return ret; @@ -91,7 +90,6 @@ bool FlightTaskAuto::_evaluateTriplets() if (!_sub_triplet_setpoint->get().current.valid) { // best we can do is to just set all waypoints to current state _prev_prev_wp = _prev_wp = _target = _next_wp = _position; - _yaw_wp = _yaw; _type = WaypointType::position; return false; } @@ -112,12 +110,12 @@ bool FlightTaskAuto::_evaluateTriplets() _sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &target(0), &target(1)); target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude); + if (_sub_triplet_setpoint->get().current.yaw_valid) { + _yaw_setpoint = _sub_triplet_setpoint->get().current.yaw; + } - _yaw_wp = _sub_triplet_setpoint->get().current.yaw; - - if (!PX4_ISFINITE(_yaw_wp)) { - _yaw_wp = _yaw; - + if (_type == WaypointType::follow_target && _sub_triplet_setpoint->get().current.yawspeed_valid) { + _yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed; } // Check if anything has changed. We do that by comparing the target @@ -201,3 +199,20 @@ void FlightTaskAuto::_setDefaultConstraints() _constraints.speed_xy = MPC_XY_CRUISE.get(); } } + +matrix::Vector2f FlightTaskAuto::_getTargetVelocityXY() +{ + // guard against any bad velocity values + const float vx = _sub_triplet_setpoint->get().current.vx; + const float vy = _sub_triplet_setpoint->get().current.vy; + bool velocity_valid = PX4_ISFINITE(vx) && PX4_ISFINITE(vy) && + _sub_triplet_setpoint->get().current.velocity_valid; + + if (velocity_valid) { + return matrix::Vector2f(vx, vy); + + } else { + // just return zero speed + return matrix::Vector2f{}; + } +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp index 603702789b..030569149e 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp @@ -56,7 +56,9 @@ enum class WaypointType : int { loiter, takeoff, land, - idle + idle, + offboard, // only part of this structure due to legacy reason. It is not used + follow_target }; class FlightTaskAuto : public FlightTask @@ -72,15 +74,14 @@ public: protected: void _setDefaultConstraints() override; float _getMaxCruiseSpeed() {return MPC_XY_CRUISE.get();} /**< getter for default cruise speed */ + matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/ matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */ matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */ matrix::Vector3f _target{}; /**< Target waypoint (local frame).*/ matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */ - float _yaw_wp{0.0f}; /**< Triplet yaw waypoint. Currently it is not a yaw-waypoint, but rather a yaw setpoint at each time stamp. */ float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */ WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */ - uORB::Subscription *_sub_home_position{nullptr}; private: diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoFollowMe.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoFollowMe.cpp new file mode 100644 index 0000000000..4487256735 --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoFollowMe.cpp @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FlightTaskAutoFollowMe.cpp + */ + +#include "FlightTaskAutoFollowMe.hpp" +#include + +using namespace matrix; + +bool FlightTaskAutoFollowMe::update() +{ + _position_setpoint = _target; + matrix::Vector2f vel_sp = _getTargetVelocityXY(); + _velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f); + return true; +} \ No newline at end of file diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoFollowMe.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoFollowMe.hpp new file mode 100644 index 0000000000..733c8d3754 --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoFollowMe.hpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FlightTaskAutoFollowMe.hpp + * + * Flight task for autonomous, gps driven follow-me mode. + */ + +#pragma once + +#include "FlightTaskAuto.hpp" + +class FlightTaskAutoFollowMe : public FlightTaskAuto +{ +public: + FlightTaskAutoFollowMe() = default; + virtual ~FlightTaskAutoFollowMe() = default; + bool update() override; +}; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index dc50d02c98..e5a317801d 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -89,12 +89,6 @@ bool FlightTaskAutoLine::update() _generateVelocitySetpoints(); } - // For now yaw-setpoint comes directly form triplets. - // TODO: In the future, however, yaw should be set in this - // task based on flag: yaw along path, yaw based on gimbal, yaw - // same as home yaw ... - _yaw_setpoint = _yaw_wp; - // update previous type _type_previous = _type; @@ -362,8 +356,8 @@ void FlightTaskAutoLine::_generateXYsetpoints() float yaw_diff = 0.0f; - if (PX4_ISFINITE(_yaw_wp)) { - yaw_diff = _wrap_pi(_yaw_wp - _yaw); + if (PX4_ISFINITE(_yaw_setpoint)) { + yaw_diff = _wrap_pi(_yaw_setpoint - _yaw); } // If yaw offset is large, only accelerate with 0.5 m/s^2.