From 6e62beb560b1e9f47ff4ee897617ff88af10b52f Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Fri, 12 Jan 2018 17:27:36 +0100 Subject: [PATCH] FlightTaskAuto: abstract class for mapping triplets to quadruple FlightTaskAuto: add type that corresponds to triplet type FligthTaskAuto: set all setpoints if invalid in xy FlightTaskAuto: cast triplet type to WaypointType FlightTaskAutoLine: class for px4 legacy auto FlightTaskAutoLine: methods prototype FlightTaskAuto: change sp to wp (=Waypoint) add params FlightTaskAutoLine: follow waypoints along line based on flight state --- src/lib/FlightTasks/CMakeLists.txt | 26 +- src/lib/FlightTasks/FlightTasks.cpp | 4 + src/lib/FlightTasks/FlightTasks.hpp | 2 + src/lib/FlightTasks/tasks/FlightTask.cpp | 10 + src/lib/FlightTasks/tasks/FlightTask.hpp | 7 + src/lib/FlightTasks/tasks/FlightTaskAuto.cpp | 182 +++++++ src/lib/FlightTasks/tasks/FlightTaskAuto.hpp | 94 ++++ .../FlightTasks/tasks/FlightTaskAutoLine.cpp | 489 ++++++++++++++++++ .../FlightTasks/tasks/FlightTaskAutoLine.hpp | 97 ++++ .../mc_pos_control/PositionControl.cpp | 58 ++- .../mc_pos_control/PositionControl.hpp | 11 +- 11 files changed, 963 insertions(+), 17 deletions(-) create mode 100644 src/lib/FlightTasks/tasks/FlightTaskAuto.cpp create mode 100644 src/lib/FlightTasks/tasks/FlightTaskAuto.hpp create mode 100644 src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp create mode 100644 src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index ea4908cc40..3c30449998 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -32,16 +32,18 @@ ############################################################################ px4_add_library(flight_tasks - tasks/FlightTask.cpp - tasks/FlightTaskManual.cpp - tasks/FlightTaskManualStabilized.cpp - tasks/FlightTaskOrbit.cpp - tasks/FlightTaskManualAltitude.cpp - tasks/FlightTaskManualAltitudeSmooth.cpp - tasks/FlightTaskManualPosition.cpp - tasks/FlightTaskManualPositionSmooth.cpp - tasks/Utility/ManualSmoothingZ.cpp - tasks/Utility/ManualSmoothingXY.cpp - SubscriptionArray.cpp - FlightTasks.cpp +tasks/FlightTask.cpp + tasks/FlightTaskManual.cpp + tasks/FlightTaskManualStabilized.cpp + tasks/FlightTaskOrbit.cpp + tasks/FlightTaskManualAltitude.cpp + tasks/FlightTaskManualAltitudeSmooth.cpp + tasks/FlightTaskManualPosition.cpp + tasks/FlightTaskManualPositionSmooth.cpp + tasks/FlightTaskAuto.cpp + tasks/FlightTaskAutoLine.cpp + tasks/Utility/ManualSmoothingZ.cpp + tasks/Utility/ManualSmoothingXY.cpp + SubscriptionArray.cpp + FlightTasks.cpp ) diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index 159471ec41..28eebea268 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/src/lib/FlightTasks/FlightTasks.cpp @@ -72,6 +72,10 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index) _current_task = new (&_task_union.sport) FlightTaskSport(); break; + case 8: + _current_task = new (&_task_union.autoLine) FlightTaskAutoLine(this, "ALN"); + break; + default: /* invalid task */ return -1; diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index e596fc6009..6344b9193c 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -47,6 +47,7 @@ #include "tasks/FlightTaskManualPosition.hpp" #include "tasks/FlightTaskManualPositionSmooth.hpp" #include "tasks/FlightTaskManualStabilized.hpp" +#include "tasks/FlightTaskAutoLine.hpp" #include "tasks/FlightTaskOrbit.hpp" #include "tasks/FlightTaskSport.hpp" @@ -139,6 +140,7 @@ private: FlightTaskManualPositionSmooth position_smooth; FlightTaskOrbit orbit; FlightTaskSport sport; + FlightTaskAutoLine autoLine; } _task_union; /**< storage for the currently active task */ FlightTask *_current_task = nullptr; diff --git a/src/lib/FlightTasks/tasks/FlightTask.cpp b/src/lib/FlightTasks/tasks/FlightTask.cpp index 7d40ebbe51..e8ed481a93 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask.cpp @@ -2,6 +2,7 @@ #include constexpr uint64_t FlightTask::_timeout; + /* First index of empty_setpoint corresponds to time-stamp and requires a finite number. */ const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}}; @@ -59,6 +60,15 @@ bool FlightTask::_evaluateVehiclePosition() _position = matrix::Vector3f(&_sub_vehicle_local_position->get().x); _velocity = matrix::Vector3f(&_sub_vehicle_local_position->get().vx); _yaw = _sub_vehicle_local_position->get().yaw; + + /* Check if reference has changed and update. */ + if (_sub_vehicle_local_position->get().ref_timestamp != _time_stamp_reference) { + map_projection_init(&_reference_position, _sub_vehicle_local_position->get().ref_lat, + _sub_vehicle_local_position->get().ref_lon); + _reference_altitude = _sub_vehicle_local_position->get().ref_alt; + _time_stamp_reference = _sub_vehicle_local_position->get().ref_timestamp; + } + return true; } else { diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index 23d5c46308..43357cdc70 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -47,6 +47,8 @@ #include #include #include +#include + #include "../SubscriptionArray.hpp" @@ -114,6 +116,7 @@ protected: hrt_abstime _time_stamp_activate = 0; /**< time stamp when task was activated */ hrt_abstime _time_stamp_current = 0; /**< time stamp at the beginning of the current task update */ hrt_abstime _time_stamp_last = 0; /**< time stamp when task was last updated */ + hrt_abstime _time_stamp_reference = 0; /**< time stamp when last reference update */ /* Current vehicle state */ matrix::Vector3f _position; /**< current vehicle position */ @@ -129,6 +132,10 @@ protected: float _yaw_setpoint; float _yawspeed_setpoint; + /* Current reference position */ + map_projection_reference_s _reference_position{}; /**< structure used to project lat/lon setpoint into local frame */ + float _reference_altitude = 0.0f; /**< altitude relative to ground */ + /** * Get the output data */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp new file mode 100644 index 0000000000..450ec38cd8 --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * 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 "FlightTaskAuto.hpp" +#include +#include +#include + +using namespace matrix; + +FlightTaskAuto::FlightTaskAuto(control::SuperBlock *parent, const char *name) : + FlightTask(parent, name), + _mc_cruise_default(this, "MPC_XY_CRUISE", false) +{} + +bool FlightTaskAuto::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 FlightTaskAuto::activate() +{ + _prev_prev_wp = _prev_wp = _target = _next_wp = _position; + _position_lock = matrix::Vector3f(NAN, NAN, NAN); + _yaw_wp = _yaw; + return FlightTask::activate(); +} + +bool FlightTaskAuto::updateInitialize() +{ + bool ret = FlightTask::updateInitialize(); + return (ret && _evaluateTriplets()); +} + +bool FlightTaskAuto::_evaluateTriplets() +{ + /* TODO: fix the issues mentioned below */ + /* We add here some conditions that are only required because + * 1. navigator continuously sends triplet during mission due to yaw setpoint. This + * should be removed in the navigator and only update once the current setpoint actually has changed. + * + * 2. navigator should be responsible to send always three valid setpoints. If there is only one setpoint, + * then previous will be set to current vehicle position and next will be set equal to setpoint. + * + * 3. navigator originally only supports gps guided maneuvers. However, it now also supports some flow-specific features + * such as land and takeoff. The navigator should use for auto takeoff/land with flow the position in xy at the moment the + * takeoff/land was initiated. Until then we do this kind of logic here. + */ + + _mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed; + + if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) { + /* Use default */ + _mc_cruise_speed = _mc_cruise_default.get(); + } + + _yaw_wp = _sub_triplet_setpoint->get().current.yaw; + _type = (WaypointType)_sub_triplet_setpoint->get().current.type; + + /* We need to have a valid current triplet */ + if (_isFinite(_sub_triplet_setpoint->get().current)) { + + /* Reset position lock */ + _position_lock = matrix::Vector3f(NAN, NAN, NAN); + + /* 1. only consider updated if current target has has changed. + * Note how bad this implementation is. In mission, in every iteration we do double operations */ + matrix::Vector3f target; + map_projection_project(&_reference_position, + _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); + + /* Dont't do any updates if the current target has not changed */ + if (fabsf(target.length() - _target.length()) < FLT_EPSILON) { + return true; + } + + /* We have a new target setpoint: update all previous setpoints */ + _target = target; + + /* Set previous to previous - 1 */ + _prev_prev_wp = _prev_wp; + + /* Check if previous is valid and update accordingly */ + if (_isFinite(_sub_triplet_setpoint->get().previous) && _sub_triplet_setpoint->get().previous.valid) { + map_projection_project(&_reference_position, _sub_triplet_setpoint->get().previous.lat, + _sub_triplet_setpoint->get().previous.lon, &_prev_wp(0), &_prev_wp(1)); + _prev_wp(2) = -(_sub_triplet_setpoint->get().previous.alt - _reference_altitude); + + + } else { + _prev_wp = _position; + } + + /* Check if previous is valid and update accordingly */ + if (_type == WaypointType::loiter) { + _next_wp = _target; + + } else if (_isFinite(_sub_triplet_setpoint->get().next) && _sub_triplet_setpoint->get().next.valid) { + map_projection_project(&_reference_position, _sub_triplet_setpoint->get().next.lat, + _sub_triplet_setpoint->get().next.lon, &_next_wp(0), &_next_wp(1)); + _next_wp(2) = -(_sub_triplet_setpoint->get().next.alt - _reference_altitude); + + } else { + _next_wp = _target; + } + + return true; + + } else if (PX4_ISFINITE(_sub_triplet_setpoint->get().current.alt)) { + /* We only have a valid altitude. Hold position in xy. */ + + _type = (WaypointType)_sub_triplet_setpoint->get().current.type; + + if (!PX4_ISFINITE(_position_lock.length())) { + _position_lock = _position; + } + + _prev_prev_wp = _prev_wp; + _prev_wp = _position_lock; + _prev_wp(2) = _target(2); + _target = _position_lock; + _target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude); + _next_wp = _target; + + return true; + + } else { + /* Reset everything */ + _prev_prev_wp = _prev_wp = _target = _next_wp = _position; + _position_lock = matrix::Vector3f(NAN, NAN, NAN); + _yaw_wp = _yaw; + + return false; + } +} + +bool FlightTaskAuto::_isFinite(const position_setpoint_s sp) +{ + return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt)); +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp new file mode 100644 index 0000000000..bdc6a22dbd --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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.hpp + * + * Map from global triplet to local quadruple. + * + */ + +#pragma once + +#include "FlightTask.hpp" +#include +#include + +/* This enum has to agree with position_setpoint_s type definition */ +enum class WaypointType : int { + position = 0, + velocity, + loiter, + takeoff, + land, + idle +}; + +class FlightTaskAuto : public FlightTask +{ +public: + FlightTaskAuto(control::SuperBlock *parent, const char *name); + + virtual ~FlightTaskAuto() = default; + + bool initializeSubscriptions(SubscriptionArray &subscription_array) override; + + bool activate() override; + + bool updateInitialize() override; + +protected: + + matrix::Vector3f _prev_prev_wp{}; /**< Triplet previous-previous triplet. This will be used for smoothing trajectories -> not used yet. */ + matrix::Vector3f _prev_wp{}; /**< Triplet previous setpoint in local frame. If not previous triplet is available, the prev_wp is set to current position. */ + matrix::Vector3f _target{}; /**< Triplet target setpoint in local frame. */ + matrix::Vector3f _next_wp{}; /**< Triplet setpoint in local frame. If no next setpoint is available, next is set to target. */ + float _yaw_wp = + 0.0f; /**< Triplet yaw waypoint. Unfortunately navigator sends yaw setpoint continuously. It would be better if a yaw setpoint is attached + to triplet waypoint. This way it would be easy for multicopter to implement features where yaw does not matter. */ + float _mc_cruise_speed = + 0.0f; /**< Cruise speed with which multicopter flies and gets set by triplet. If no valid, default cruise speed is used. */ + WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */ + +private: + control::BlockParamFloat _mc_cruise_default; /**< Default mc cruise speed*/ + + matrix::Vector3f _position_lock{}; /**< Position lock is NAN except when target lat/lon are not finite. */ + map_projection_reference_s _reference; /**< Reference frame from global to local */ + + uORB::Subscription *_sub_triplet_setpoint{nullptr}; + + bool _evaluateTriplets(); /**< Checks and sets triplets */ + bool _isFinite(const position_setpoint_s sp); + void _updateReference(); +}; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp new file mode 100644 index 0000000000..8d76ac5c49 --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -0,0 +1,489 @@ +/**************************************************************************** + * + * 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 FlightAutoLine.cpp + */ + +#include "FlightTaskAutoLine.hpp" +#include + +using namespace matrix; + +#define SIGMA_SINGLE_OP 0.000001f +#define SIGMA_NORM 0.001f + +FlightTaskAutoLine::FlightTaskAutoLine(control::SuperBlock *parent, const char *name) : + FlightTaskAuto(parent, name), + _land_speed(parent, "MPC_LAND_SPEED", false), + _vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false), + _vel_max_down(parent, "MPC_Z_VEL_MAX_DN", false), + _acc_max_xy(parent, "MPC_ACC_HOR_MAX", false), + _acc_xy(parent, "MPC_ACC_HOR", false), + _acc_max_up(parent, "MPC_ACC_UP_MAX", false), + _acc_max_down(parent, "MPC_ACC_DOWN_MAX", false), + _cruise_speed_90(parent, "MPC_CRUISE_90", false), + _nav_rad(parent, "NAV_ACC_RAD", false), + _mis_yaw_error(parent, "MIS_YAW_ERR", false) +{} + +bool FlightTaskAutoLine::activate() +{ + _vel_sp_xy = matrix::Vector2f(&_velocity(0)); + _pos_sp_xy = matrix::Vector2f(&_position(0)); + _vel_sp_z = _velocity(2); + _pos_sp_z = _position(2); + _destination = _target; + _origin = _prev_wp; + _speed_at_target = 0.0f; + + return FlightTaskAuto::activate(); +} + +bool FlightTaskAutoLine::update() +{ + if (_type == WaypointType::idle) { + + _generateIdleSetpoints(); + + } else if (_type == WaypointType::land) { + + _generateLandSetpoints(); + + } else if (_type == WaypointType::loiter || _type == WaypointType::position) { + + _generateSetpoints(); + + } else if (_type == WaypointType::takeoff) { + + _generateTakeoffSetpoints(); + + } else if (_type == WaypointType::velocity) { + + _generateVelocitySetpoints(); + } + + /* Send setpoints */ + _setVelocitySetpoint(Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), _vel_sp_z)); + _setPositionSetpoint(Vector3f(_pos_sp_xy(0), _pos_sp_xy(1), _pos_sp_z)); + _setYawSetpoint(_yaw_wp); + + return true; +} + +void FlightTaskAutoLine::_generateIdleSetpoints() +{ + /* Send zero thrust setpoint */ + _vel_sp_xy = matrix::Vector2f(NAN, NAN); + _vel_sp_z = NAN; + _pos_sp_xy = matrix::Vector2f(NAN, NAN); + _pos_sp_z = NAN; + _setThrustSetpoint(Vector3f(0.0f, 0.0f, 0.0f)); +} + +void FlightTaskAutoLine::_generateLandSetpoints() +{ + _pos_sp_xy = Vector2f(&_target(0)); + _pos_sp_z = NAN; + _vel_sp_xy *= NAN; + _vel_sp_z = _land_speed.get(); +} + +void FlightTaskAutoLine::_generateTakeoffSetpoints() +{ + _pos_sp_xy = Vector2f(&_target(0)); + _pos_sp_z = _target(2); + _vel_sp_xy.zero(); + _vel_sp_z = 0.0f; +} + +void FlightTaskAutoLine::_generateSetpoints() +{ + _updateInternalWaypoints(); + _generateAltitudeSetpoints(); + _generateXYsetpoints(); +} + +void FlightTaskAutoLine::_updateInternalWaypoints() +{ + + if (_type == WaypointType::loiter) { + /* If loiter, then we want to stop at loiter anyway. Hence, just + * set next waypoint to target. + */ + _next_wp = _target; + } + + /* Adjust destination and origin based on current vehicle state */ + Vector2f u_prev_to_target = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero(); + Vector2f pos_to_target = Vector2f(&(_target - _position)(0)); + Vector2f prev_to_pos = Vector2f(&(_position - _prev_wp)(0)); + Vector2f closest_pt = Vector2f(&_prev_wp(0)) + u_prev_to_target * (prev_to_pos * u_prev_to_target); + + if (u_prev_to_target * pos_to_target < 0.0f) { + + /* Target is behind */ + if (_current_state != State::target_behind) { + + _destination = _target; + _origin = _position; + _current_state = State::target_behind; + + float angle = 2.0f; + _speed_at_target = 0.0f; + + /* angle = cos(x) + 1.0 + * angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 */ + + if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f && + (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) { + + angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero() + * Vector2f(&(_destination - _next_wp)(0)).unit_or_zero() + + 1.0f; + _speed_at_target = _getVelcoityFromAngle(angle); + } + } + + } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) { + + /* current position more than cruise speed in front of previous setpoint. */ + if (_current_state != State::previous_infront) { + _destination = _prev_wp; + _origin = _position; + _current_state = State::previous_infront; + + float angle = 2.0f; + _speed_at_target = 0.0f; + + /* angle = cos(x) + 1.0 + * angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 */ + if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f && + (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) { + + angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero() + * Vector2f(&(_destination - _next_wp)(0)).unit_or_zero() + + 1.0f; + _speed_at_target = _getVelcoityFromAngle(angle); + } + + } + + } else if (Vector2f(Vector2f(&_position(0)) - closest_pt).length() > _mc_cruise_speed) { + + /* Vehicle is more than cruise speed off track */ + if (_current_state != State::offtrack) { + _destination = matrix::Vector3f(closest_pt(0), closest_pt(1), _target(2)); + _origin = _position; + _current_state = State::offtrack; + + float angle = 2.0f; + _speed_at_target = 0.0f; + + /* angle = cos(x) + 1.0 + * angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 */ + if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f && + (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) { + + angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero() + * Vector2f(&(_destination - _next_wp)(0)).unit_or_zero() + + 1.0f; + _speed_at_target = _getVelcoityFromAngle(angle); + } + + } + + } else { + + if ((_target - _destination).length() > 0.01f) { + _destination = _target; + _origin = _prev_wp; + _current_state = State::none; + + float angle = 2.0f; + _speed_at_target = 0.0f; + + + /* angle = cos(x) + 1.0 + * angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 */ + if (Vector2f(&(_destination - _next_wp)(0)).length() > 0.001f && + (Vector2f(&(_destination - _origin)(0)).length() > _nav_rad.get())) { + + angle = + Vector2f(&(_destination - _origin)(0)).unit_or_zero() + * Vector2f(&(_destination - _next_wp)(0)).unit_or_zero() + + 1.0f; + _speed_at_target = _getVelcoityFromAngle(angle); + } + } + } +} + +void FlightTaskAutoLine::_generateXYsetpoints() +{ + + Vector2f pos_sp_to_dest = Vector2f(&_target(0)) - _pos_sp_xy; + const bool has_reached_altitude = fabsf(_destination(2) - _position(2)) < _nav_rad.get(); + + if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _nav_rad.get()) || + (!has_reached_altitude && pos_sp_to_dest.length() < _nav_rad.get())) { + + /* Vehicle reached target in xy. Lock position */ + _pos_sp_xy = Vector2f(&_destination(0)); + _vel_sp_xy.zero(); + + } else { + + Vector2f u_prev_to_dest = Vector2f(&(_destination - _origin)(0)).unit_or_zero(); + Vector2f prev_to_pos(&(_position - _origin)(0)); + Vector2f closest_pt = Vector2f(&_origin(0)) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest); + Vector2f closest_to_dest = Vector2f(&(_destination - _position)(0)); + Vector2f prev_to_dest = Vector2f(&(_destination - _origin)(0)); + float speed_sp_track = _mc_cruise_speed; + float speed_sp_prev_track = math::max(_vel_sp_xy * u_prev_to_dest, 0.0f); + + /* Distance to target when brake should occur. The assumption is made that + * 1.5 * cruising speed is enough to break. */ + float target_threshold = 1.5f * _mc_cruise_speed; + float speed_threshold = _mc_cruise_speed; + const float threshold_max = target_threshold; + + if (target_threshold < 0.5f * prev_to_dest.length()) { + target_threshold = 0.5f * prev_to_dest.length(); + } + + /* Compute maximum speed at target threshold */ + if (threshold_max > _nav_rad.get()) { + float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _nav_rad.get()); + speed_threshold = m * (target_threshold - _nav_rad.get()) + _speed_at_target; // speed at transition + } + + /* Either accelerate or decelerate */ + if (closest_to_dest.length() < target_threshold) { + + /* Vehicle is close to destination. Start to decelerate */ + + if (!has_reached_altitude) { + /* Altitude is not reached yet. Vehicle has to stop first before proceeding */ + _speed_at_target = 0.0f; + } + + float acceptance_radius = _nav_rad.get(); + + if (_speed_at_target < 0.01f) { + acceptance_radius = 0.0f; + } + + if ((target_threshold - acceptance_radius) >= SIGMA_NORM) { + + float m = (speed_threshold - _speed_at_target) / (target_threshold - acceptance_radius); + speed_sp_track = m * (closest_to_dest.length() - acceptance_radius) + _speed_at_target; // speed at transition + + } else { + speed_sp_track = _speed_at_target; + } + + /* If we are close to target and the previous speed setpoint along track was smaller than + * current speed setpoint along track, then take over the previous one. + * This ensures smoothness since we anyway want to slow down + */ + if ((speed_sp_prev_track < speed_sp_track) + && (speed_sp_track * speed_sp_prev_track > 0.0f) + && (speed_sp_prev_track > _speed_at_target)) { + speed_sp_track = speed_sp_prev_track; + } + + } else { + + /* Vehicle is still far from destination. Accelerate or keep maximum target speed */ + float acc_track = (speed_sp_track - speed_sp_prev_track) / _deltatime; + float yaw_diff = _wrap_pi(_yaw_wp - _yaw); + + /* If yaw offset is large, only accelerate with 0.5 m/s^2 */ + float acc_max = (fabsf(yaw_diff) > math::radians(_mis_yaw_error.get())) ? 0.5f : _acc_xy.get(); + + if (acc_track > acc_max) { + speed_sp_track = acc_max * _deltatime + speed_sp_prev_track; + } + } + + speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed); + + _pos_sp_xy = closest_pt; + _vel_sp_xy = u_prev_to_dest * speed_sp_track; + } +} + +void FlightTaskAutoLine::_generateAltitudeSetpoints() +{ + + /* Total distance between previous and target setpoint */ + const float dist = fabsf(_destination(2) - _origin(2)); + + /* If target has not been reached, then compute setpoint depending on maximum velocity */ + if ((dist > SIGMA_NORM) && (fabsf(_position(2) - _destination(2)) > 0.1f)) { + + /* get various distances */ + const float dist_to_prev = fabsf(_position(2) - _origin(2)); + const float dist_to_target = fabsf(_destination(2) - _position(2)); + + /* check sign */ + const bool flying_upward = _destination(2) < _position(2); + + /* Speed at threshold is by default maximum speed. Threshold defines + * the point in z at which vehicle slows down to reach target altitude. */ + float speed_sp = (flying_upward) ? _vel_max_up.get() : _vel_max_down.get(); + + /* target threshold defines the distance to target(2) at which + * the vehicle starts to slow down to approach the target smoothly */ + float target_threshold = speed_sp * 1.5f; + + /* If the total distance in z is NOT 2x distance of target_threshold, we + * will need to adjust the final_velocity in z */ + const bool is_2_target_threshold = dist >= 2.0f * target_threshold; + const float min_vel = 0.2f; // minimum velocity: this is needed since estimation is not perfect + const float slope = (speed_sp - min_vel) / (target_threshold); /* defines the the acceleration when slowing down */ + + if (!is_2_target_threshold) { + /* adjust final_velocity since we are already are close + * to target and therefore it is not necessary to accelerate + * up to full speed + */ + target_threshold = dist * 0.5f; + /* get the velocity at target_threshold */ + speed_sp = slope * (target_threshold) + min_vel; + } + + /* we want to slow down */ + if (dist_to_target < target_threshold) { + + speed_sp = slope * dist_to_target + min_vel; + + } else if (dist_to_prev < target_threshold) { + /* we want to accelerate */ + + const float acc = (speed_sp - fabsf(_vel_sp_z)) / _deltatime; + const float acc_max = (flying_upward) ? (_acc_max_up.get() * 0.5f) : (_acc_max_down.get() * 0.5f); + + if (acc > acc_max) { + speed_sp = acc_max * _deltatime + fabsf(_vel_sp_z); + } + } + + /* make sure vel_sp_z is always positive */ + if (speed_sp < 0.0f) { + PX4_WARN("speed cannot be smaller than 0"); + speed_sp = 0.0f; + } + + /* get the sign of vel_sp_z */ + _vel_sp_z = (flying_upward) ? -speed_sp : speed_sp; + _pos_sp_z = NAN; // We don't care about position setpoint */ + + } else { + + /* Vehicle reached desired target altitude */ + _vel_sp_z = 0.0f; + _pos_sp_z = _target(2); + } +} +void FlightTaskAutoLine::_generateVelocitySetpoints() +{ + /* TODO: Remove velocity force logic from navigator, since + * navigator should only send out waypoints. */ + _vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed; + _vel_sp_z = 0.0f; + _pos_sp_z = _position(2); + _pos_sp_xy = Vector2f(NAN, NAN); +} + +float FlightTaskAutoLine::_getVelcoityFromAngle(const float angle) +{ + /* Minimum cruise speed when passing waypoint */ + float min_cruise_speed = 0.0f; + + /* Make sure that cruise speed is larger than minimum*/ + if ((_mc_cruise_speed - min_cruise_speed) < SIGMA_NORM) { + return _mc_cruise_speed; + } + + /* Middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle of 90degrees. + * It needs to be always larger than minimum cruise speed. */ + float middle_cruise_speed = _cruise_speed_90.get(); + + if ((middle_cruise_speed - min_cruise_speed) < SIGMA_NORM) { + middle_cruise_speed = min_cruise_speed + SIGMA_NORM; + } + + if ((_mc_cruise_speed - middle_cruise_speed) < SIGMA_NORM) { + middle_cruise_speed = (_mc_cruise_speed + min_cruise_speed) * 0.5f; + } + + /* If middle cruise speed is exactly in the middle, then compute + * speed linearly + */ + bool use_linear_approach = false; + + if (((_mc_cruise_speed + min_cruise_speed) * 0.5f) - middle_cruise_speed < SIGMA_NORM) { + use_linear_approach = true; + } + + /* Compute speed sp at target */ + float speed_close; + + if (use_linear_approach) { + + /* velocity close to target adjusted to angle + * vel_close = m*x+q + */ + float slope = -(_mc_cruise_speed - min_cruise_speed) / 2.0f; + speed_close = slope * angle + _mc_cruise_speed; + + } else { + + /* Speed close to target adjusted to angle x. + * speed_close = a *b ^x + c; where at angle x = 0 -> speed_close = cruise; angle x = 1 -> speed_close = middle_cruise_speed (this means that at 90degrees + * the velocity at target is middle_cruise_speed); + * angle x = 2 -> speed_close = min_cruising_speed */ + + /* from maximum cruise speed, minimum cruise speed and middle cruise speed compute constants a, b and c */ + float a = -((middle_cruise_speed - _mc_cruise_speed) * (middle_cruise_speed - _mc_cruise_speed)) + / (2.0f * middle_cruise_speed - _mc_cruise_speed - min_cruise_speed); + float c = _mc_cruise_speed - a; + float b = (middle_cruise_speed - c) / a; + speed_close = a * powf(b, angle) + c; + } + + /* speed_close needs to be in between max and min */ + return math::constrain(speed_close, min_cruise_speed, _mc_cruise_speed); +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp new file mode 100644 index 0000000000..e3a8ca3f0d --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 FlightTaskAutoLine.hpp + * + * Flight task for autonomous, gps driven mode. The vehicle flies + * along a straight line in between waypoints. + */ + +#pragma once + +#include "FlightTaskAuto.hpp" + +class FlightTaskAutoLine : public FlightTaskAuto +{ +public: + FlightTaskAutoLine(control::SuperBlock *parent, const char *name); + + virtual ~FlightTaskAutoLine() = default; + + bool activate() override; + + bool update() override; + +protected: + + matrix::Vector2f _vel_sp_xy{}; + matrix::Vector2f _pos_sp_xy{}; + float _vel_sp_z = 0.0f; + float _pos_sp_z = 0.0f; + + matrix::Vector3f _destination{}; /**< Current target. Is not necessarily the same as triplet target. */ + matrix::Vector3f _origin{}; /**< Previous waypoint. Is not necessarily the same as triplet previous. */ + float _speed_at_target = 0.0f; /**< Desired velocity at target. */ + + enum class State { + offtrack, /**< Vehicle is more than cruise speed away from track */ + target_behind, /**< Vehicle is in front of target. */ + previous_infront, /**< Vehilce is behind previous waypoint.*/ + none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */ + }; + State _current_state{State::none}; + + control::BlockParamFloat _land_speed; /**< Downward speed during landing. */ + control::BlockParamFloat _vel_max_up; /**< Maximum upward velocity. */ + control::BlockParamFloat _vel_max_down; /**< Maximum downward velocity. */ + control::BlockParamFloat _acc_max_xy; /**< Maximum acceleration in hover. */ + control::BlockParamFloat _acc_xy; /**< Maximum acceleration from hover to fast forward flight. */ + control::BlockParamFloat _acc_max_up; /**< Max acceleration up. */ + control::BlockParamFloat _acc_max_down; /**< Max acceleration down. */ + control::BlockParamFloat _cruise_speed_90; /**< Speed when angle is 90 degrees between prev-current/current-next. */ + + /* None-position control params */ + control::BlockParamFloat _nav_rad; /**< Radius that is used by navigator that defines when to update triplets */ + control::BlockParamFloat _mis_yaw_error; /**< Yaw threshold during mission to consider yaw as accepted */ + + void _generateIdleSetpoints(); + void _generateLandSetpoints(); + void _generateVelocitySetpoints(); + void _generateTakeoffSetpoints(); + void _updateInternalWaypoints(); + void _generateSetpoints(); /**< Generate setpoints during auto tracking */ + void _generateAltitudeSetpoints(); + void _generateXYsetpoints(); + float _getVelcoityFromAngle(const float angle); /** Computes the speed at target depending on angle */ +}; diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index 4c23c3e4b2..b09ba6852c 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -108,7 +108,23 @@ void PositionControl::generateThrustYawSetpoint(const float &dt) */ if (!_skipController) { _positionController(); + + if (_smooth_takeoff) { + _setTakeoffVelocity(dt); + + } else { + _takeoff_vel_sp = 0.5f; // Reset to pointing downwards. + } + _velocityController(dt); + + } else { + if (_smooth_takeoff) { + _setTakeoffThrust(dt); + + } else { + _takeoff_vel_sp = 0.5f; // Reset to pointing downwards. + } } } @@ -119,6 +135,13 @@ void PositionControl::_interfaceMapping() * do not require control. */ + if (PX4_ISFINITE(_pos_sp(2)) && _smooth_takeoff) { + _takeoff_max_speed = 0.6f; + + } else { + _takeoff_max_speed = _VelMaxZ[0]; + } + /* Loop through x,y and z components of all setpoints. */ for (int i = 0; i <= 2; i++) { @@ -176,11 +199,11 @@ void PositionControl::_positionController() _vel_sp = (_pos_sp - _pos).emult(Pp) + _vel_sp; /* Make sure velocity setpoint is constrained in all directions (xyz). */ - float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1)); + Vector2f vel_sp_xy(&_vel_sp(0)); - if (vel_norm_xy > _VelMaxXY) { - _vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_norm_xy; - _vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_norm_xy; + if (vel_sp_xy.length() >= _VelMaxXY) { + _vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_sp_xy.length(); + _vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_sp_xy.length(); } /* Saturate velocity in D-direction */ @@ -311,6 +334,33 @@ void PositionControl::_updateParams() } } +void PositionControl::_setTakeoffVelocity(const float dt) +{ + + /* Limit velocity setpoint to maximum takeoff velocity */ + _vel_sp(2) = math::max(_vel_sp(2), -_takeoff_max_speed); + /* Smooth takeoff is achieved once target velocity is reached. (NED frame). */ + _smooth_takeoff = _takeoff_vel_sp >= _vel_sp(2); + /* ramp vertical velocity limit up to takeoff speed */ + _takeoff_vel_sp += _vel_sp(2) * dt / _TakeoffRampTime; + /* limit vertical velocity to the current ramp value */ + _vel_sp(2) = math::max(_vel_sp(2), _takeoff_vel_sp); + +} + +void PositionControl::_setTakeoffThrust(const float dt) +{ + + /* Smooth takeoff is achieved once target velocity is reached. (NED frame). */ + _smooth_takeoff = _takeoff_vel_sp >= _thr_sp(2); + /* ramp vertical velocity limit up to takeoff speed */ + _takeoff_vel_sp += _thr_sp(2) * dt / _TakeoffRampTime; + /* limit vertical velocity to the current ramp value */ + _thr_sp(2) = math::max(_thr_sp(2), _takeoff_vel_sp); + + +} + void PositionControl::_setParams() { param_get(_Pxy_h, &Pp(0)); diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp index ba9dd97685..dea1c8af62 100644 --- a/src/modules/mc_pos_control/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl.hpp @@ -64,7 +64,8 @@ public: ~PositionControl() {}; - void updateState(const vehicle_local_position_s &state, const matrix::Vector3f &vel_dot); + void updateState(const vehicle_local_position_s &state, const matrix::Vector3f &vel_dot, + const bool smooth_takeoff); void updateSetpoint(const vehicle_local_position_setpoint_s &setpoint); void updateConstraints(const Controller::Constraints &constraints); void generateThrustYawSetpoint(const float &dt); @@ -74,6 +75,7 @@ public: float getYawspeedSetpoint() {return _yawspeed_sp;} matrix::Vector3f getVelSp() {return _vel_sp;} matrix::Vector3f getPosSp() {return _pos_sp;} + bool getSmoothTakeoff() {return _smooth_takeoff;}; private: @@ -91,6 +93,9 @@ private: matrix::Vector3f _thr_sp{}; float _yaw_sp{}; float _yawspeed_sp{}; + float _takeoff_vel_sp = 0.5f; // starts positive (NED frame) + float _takeoff_max_speed = 1.0f; + bool _smooth_takeoff{false}; /* Other variables */ matrix::Vector3f _thr_int{}; @@ -127,6 +132,8 @@ private: }; Limits _ThrustLimit; float _ThrHover{0.5f}; + + float _ThrLimit[2]; //index 0: max, index 1: min bool _skipController{false}; /* Helper methods */ @@ -135,4 +142,6 @@ private: void _velocityController(const float &dt); void _updateParams(); void _setParams(); + void _setTakeoffVelocity(const float dt); + void _setTakeoffThrust(const float dt); };