From 1099dd8c7d57880b099f72b480ca1d14b4d5c648 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 5 Apr 2018 16:31:03 +0200 Subject: [PATCH] FlightTaskOffboard: first commit similar in logic to legacy code FlightTaskOffboard: ensure supported offboard setpoints --- src/lib/FlightTasks/CMakeLists.txt | 1 + src/lib/FlightTasks/FlightTasks.cpp | 4 + src/lib/FlightTasks/FlightTasks.hpp | 3 + .../FlightTasks/tasks/FlightTaskOffboard.cpp | 169 ++++++++++++++++++ .../FlightTasks/tasks/FlightTaskOffboard.hpp | 55 ++++++ 5 files changed, 232 insertions(+) create mode 100644 src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp create mode 100644 src/lib/FlightTasks/tasks/FlightTaskOffboard.hpp diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index 66aaf56645..d237ed9eb9 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -43,6 +43,7 @@ tasks/FlightTask.cpp tasks/FlightTaskAuto.cpp tasks/FlightTaskAutoLine.cpp tasks/FlightTaskAutoFollowMe.cpp + tasks/FlightTaskOffboard.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 38cec0092e..99bb1526d9 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/src/lib/FlightTasks/FlightTasks.cpp @@ -90,6 +90,10 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index) _current_task = new (&_task_union.autoFollowMe) FlightTaskAutoFollowMe(); break; + case FlightTaskIndex::Offboard: + _current_task = new (&_task_union.offboard) FlightTaskOffboard(); + break; + default: /* invalid task */ return -1; diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index a2343108cd..92765815f5 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -51,6 +51,7 @@ #include "tasks/FlightTaskAutoFollowMe.hpp" #include "tasks/FlightTaskOrbit.hpp" #include "tasks/FlightTaskSport.hpp" +#include "tasks/FlightTaskOffboard.hpp" #include "SubscriptionArray.hpp" @@ -67,6 +68,7 @@ enum class FlightTaskIndex : int { Sport, AutoLine, AutoFollowMe, + Offboard, Count // number of tasks }; @@ -151,6 +153,7 @@ private: FlightTaskSport sport; FlightTaskAutoLine autoLine; FlightTaskAutoFollowMe autoFollowMe; + FlightTaskOffboard offboard; } _task_union; /**< storage for the currently active task */ FlightTask *_current_task = nullptr; diff --git a/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp b/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp new file mode 100644 index 0000000000..67e3b6180a --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * 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 FlightTaskAuto.cpp + */ +#include "FlightTaskOffboard.hpp" +#include +#include + +using namespace matrix; + +bool FlightTaskOffboard::initializeSubscriptions(SubscriptionArray &subscription_array) +{ + if (!FlightTask::initializeSubscriptions(subscription_array)) { + return false; + } + + if (!subscription_array.get(ORB_ID(position_setpoint_triplet), _sub_triplet_setpoint)) { + return false; + } + + return true; +} + +bool FlightTaskOffboard::update() +{ + if (!_sub_triplet_setpoint->get().current.valid) { + _resetSetpoints(); + _position_setpoint = _position; + return false; + } + + // reset setpoint for every loop + _resetSetpoints(); + + // Possible inputs: + // 1. position setpoint + // 2. position setpoint + velocity setpoint (velocity used as feedforward) + // 3. velocity setpoint + // 4. acceleration setpoint -> this will be mapped to normalized thrust setpoint because acceleration is not supported + + const bool position_ctrl_xy = _sub_triplet_setpoint->get().current.position_valid + && _sub_vehicle_local_position->get().xy_valid; + const bool position_ctrl_z = _sub_triplet_setpoint->get().current.alt_valid + && _sub_vehicle_local_position->get().z_valid; + const bool velocity_ctrl_xy = _sub_triplet_setpoint->get().current.velocity_valid + && _sub_vehicle_local_position->get().v_xy_valid; + const bool velocity_ctrl_z = _sub_triplet_setpoint->get().current.velocity_valid + && _sub_vehicle_local_position->get().v_z_valid; + const bool feedforward_ctrl_xy = position_ctrl_xy && velocity_ctrl_xy; + const bool feedforward_ctrl_z = position_ctrl_z && velocity_ctrl_z; + const bool acceleration_ctrl = _sub_triplet_setpoint->get().current.acceleration_valid; + + // if nothing is valid in xy, then exit offboard + if (!(position_ctrl_xy || velocity_ctrl_xy || acceleration_ctrl)) { + return false; + } + + // if nothing is valid in z, then exit offboard + if (!(position_ctrl_z || velocity_ctrl_z || acceleration_ctrl)) { + return false; + } + + // XY-direction + if (feedforward_ctrl_xy) { + _position_setpoint(0) = _sub_triplet_setpoint->get().current.x; + _position_setpoint(1) = _sub_triplet_setpoint->get().current.y; + _velocity_setpoint(0) = _sub_triplet_setpoint->get().current.vx; + _velocity_setpoint(1) = _sub_triplet_setpoint->get().current.vy; + + } else if (position_ctrl_xy) { + _position_setpoint(0) = _sub_triplet_setpoint->get().current.x; + _position_setpoint(1) = _sub_triplet_setpoint->get().current.y; + + } else if (velocity_ctrl_xy) { + + if (_sub_triplet_setpoint->get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) { + // in local frame: don't require any transformation + _velocity_setpoint(0) = _sub_triplet_setpoint->get().current.vx; + _velocity_setpoint(1) = _sub_triplet_setpoint->get().current.vy; + + } else if (_sub_triplet_setpoint->get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) { + // in body frame: need to transorm first + // Note, this transformation is wrong because body-xy is not neccessarily on the same plane as locale-xy + _velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint->get().current.vx - sinf( + _yaw) * _sub_triplet_setpoint->get().current.vy; + _velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint->get().current.vx + cosf( + _yaw) * _sub_triplet_setpoint->get().current.vy; + + } else { + // no valid frame + return false; + } + } + + // Z-direction + + if (feedforward_ctrl_z) { + _position_setpoint(2) = _sub_triplet_setpoint->get().current.z; + _velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz; + + } else if (position_ctrl_z) { + _position_setpoint(2) = _sub_triplet_setpoint->get().current.z; + + } else if (velocity_ctrl_z) { + _velocity_setpoint(2) = _sub_triplet_setpoint->get().current.vz; + } + + // Yaw / Yaw-speed + + if (_sub_triplet_setpoint->get().current.yaw_valid) { + // yaw control required + _yaw_setpoint = _sub_triplet_setpoint->get().current.yaw; + + if (_sub_triplet_setpoint->get().current.yawspeed_valid) { + // yawspeed is used as feedforward + _yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed; + } + + } else if (_sub_triplet_setpoint->get().current.yawspeed_valid) { + // only yawspeed required + _yawspeed_setpoint = _sub_triplet_setpoint->get().current.yawspeed; + // set yaw setpoint to NAN since not used + _yaw_setpoint = NAN; + + } + + // Acceleration + // Note: this is not supported yet and will be mapped to normalized thrust directly. + + if (_sub_triplet_setpoint->get().current.acceleration_valid) { + _thrust_setpoint(0) = _sub_triplet_setpoint->get().current.a_x; + _thrust_setpoint(1) = _sub_triplet_setpoint->get().current.a_y; + _thrust_setpoint(2) = _sub_triplet_setpoint->get().current.a_z; + } + + return true; + +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskOffboard.hpp b/src/lib/FlightTasks/tasks/FlightTaskOffboard.hpp new file mode 100644 index 0000000000..8f250eecf1 --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskOffboard.hpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 FlightTaskOffboard.hpp + */ + +#pragma once + +#include "FlightTask.hpp" +#include +#include + +class FlightTaskOffboard : public FlightTask +{ +public: + FlightTaskOffboard() = default; + + virtual ~FlightTaskOffboard() = default; + bool initializeSubscriptions(SubscriptionArray &subscription_array) override; + bool update() override; + +protected: + uORB::Subscription *_sub_triplet_setpoint{nullptr}; +};