diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index b80abca05836..4d7530241b15 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -41,6 +41,8 @@ px4_add_module( 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 diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index 159471ec41b6..e0a4050efaa1 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 FlightTaskIndex::AutoLine: + _current_task = new (&_task_union.autoLine) FlightTaskAutoLine(); + break; + default: /* invalid task */ return -1; diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index e596fc6009b2..c6cb1301b823 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" @@ -63,6 +64,7 @@ enum class FlightTaskIndex : int { PositionSmooth, Orbit, Sport, + AutoLine, Count // number of tasks }; @@ -139,6 +141,7 @@ class FlightTasks 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 7d40ebbe5114..fcd16c2a6e73 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask.cpp @@ -27,7 +27,7 @@ bool FlightTask::updateInitialize() _time = (_time_stamp_current - _time_stamp_activate) / 1e6f; _deltatime = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f; _time_stamp_last = _time_stamp_current; - return _evaluateVehiclePosition(); + return _evaluateVehicleLocalPosition(); } const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint() @@ -53,7 +53,7 @@ void FlightTask::_resetSetpoints() _yaw_setpoint = _yawspeed_setpoint = NAN; } -bool FlightTask::_evaluateVehiclePosition() +bool FlightTask::_evaluateVehicleLocalPosition() { if ((_time_stamp_current - _sub_vehicle_local_position->get().timestamp) < _timeout) { _position = matrix::Vector3f(&_sub_vehicle_local_position->get().x); diff --git a/src/lib/FlightTasks/tasks/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask.hpp index 23d5c463083c..7074df8e8d74 100644 --- a/src/lib/FlightTasks/tasks/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask.hpp @@ -47,10 +47,8 @@ #include #include #include - #include "../SubscriptionArray.hpp" - class FlightTask : public ModuleParams { public: @@ -134,8 +132,6 @@ class FlightTask : public ModuleParams */ void _resetSetpoints(); -private: uORB::Subscription *_sub_vehicle_local_position{nullptr}; - - bool _evaluateVehiclePosition(); + bool _evaluateVehicleLocalPosition(); }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp new file mode 100644 index 000000000000..96ddb25c1051 --- /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 + +using namespace matrix; + +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; + _yaw_wp = _yaw; + return FlightTask::activate(); +} + +bool FlightTaskAuto::updateInitialize() +{ + bool ret = FlightTask::updateInitialize(); + _evaluateVehicleGlobalPosition(); + 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 updates if 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. + + 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; + } + + _type = (WaypointType)_sub_triplet_setpoint->get().current.type; + // always update cruise speed since that can change without waypoint changes + _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(); + } + + // get target waypoint. + 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); + + + _yaw_wp = _sub_triplet_setpoint->get().current.yaw; + + if (!PX4_ISFINITE(_yaw_wp)) { + _yaw_wp = _yaw; + + } + + // Check if anything has changed. We do that by comparing the target + // setpoint to the previous target. + // TODO This is a hack and it would be much better if the navigator only sends out a waypoints once tthey have changed. + + // dont't do any updates if the current target has not changed + if (!(fabsf(target(0) - _target(0)) > 0.001f || fabsf(target(1) - _target(1)) > 0.001f + || fabsf(target(2) - _target(2)) > 0.001f)) { + // nothing has changed: just keep old waypoints + return true; + } + + // update all waypoints + _target = target; + + if (!PX4_ISFINITE(_target(0)) || !PX4_ISFINITE(_target(1))) { + // Horizontal target is not finite. */ + _target(0) = _position(0); + _target(1) = _position(1); + } + + if (!PX4_ISFINITE(_target(2))) { + _target(2) = _position(2); + } + + _prev_prev_wp = _prev_wp; + + 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; + } + + 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; +} + +bool FlightTaskAuto::_isFinite(const position_setpoint_s sp) +{ + return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt)); +} + +void FlightTaskAuto::_evaluateVehicleGlobalPosition() +{ + FlightTask::_evaluateVehicleLocalPosition(); + + // 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; + } +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp new file mode 100644 index 000000000000..dae425b7a7c3 --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * 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.hpp + * + * Map from global triplet to local quadruple. + */ + +#pragma once + +#include "FlightTask.hpp" +#include +#include +#include + +/** + * This enum has to agree with position_setpoint_s type definition + * The only reason for not using the struct position_setpoint is because + * of the size + */ +enum class WaypointType : int { + position = 0, + velocity, + loiter, + takeoff, + land, + idle +}; + +class FlightTaskAuto : public FlightTask +{ +public: + FlightTaskAuto() = default; + + virtual ~FlightTaskAuto() = default; + bool initializeSubscriptions(SubscriptionArray &subscription_array) override; + bool activate() override; + bool updateInitialize() override; + +protected: + 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. */ + +private: + uORB::Subscription *_sub_triplet_setpoint{nullptr}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _mc_cruise_default); /**< Default mc cruise speed.*/ + + map_projection_reference_s _reference; /**< Reference frame from global to local. */ + + 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. */ + hrt_abstime _time_stamp_reference = 0; /**< time stamp when last reference update occured. */ + + bool _evaluateTriplets(); /**< Checks and sets triplets. */ + bool _isFinite(const position_setpoint_s sp); /**< Checks if all waypoint triplets are finite. */ + void _updateReference(); /**< Updates the local reference. */ + void _evaluateVehicleGlobalPosition(); /**< Checks if global position is valid. */ +}; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp new file mode 100644 index 000000000000..c965618fc881 --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -0,0 +1,495 @@ +/**************************************************************************** + * + * 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 + +bool FlightTaskAutoLine::activate() +{ + bool ret = FlightTaskAuto::activate(); + _reset(); + return ret; +} + +bool FlightTaskAutoLine::update() +{ + bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position; + bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position; + + // 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state. + if (follow_line && !follow_line_prev) { + _reset(); + } + + // The only time a thrust set-point is sent out is during + // idle. Hence, reset thrust set-point to NAN in case the + // vehicle exits idle. + + if (_type_previous == WaypointType::idle) { + _thrust_setpoint = Vector3f(NAN, NAN, NAN); + } + + if (_type == WaypointType::idle) { + _generateIdleSetpoints(); + + } else if (_type == WaypointType::land) { + _generateLandSetpoints(); + + } else if (follow_line) { + _generateSetpoints(); + + } else if (_type == WaypointType::takeoff) { + _generateTakeoffSetpoints(); + + } else if (_type == WaypointType::velocity) { + _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; + + return true; +} + +void FlightTaskAutoLine::_reset() +{ + // Set setpoints equal current state. + _velocity_setpoint = _velocity; + _position_setpoint = _position; + _destination = _target; + _origin = _prev_wp; + _speed_at_target = 0.0f; +} + +void FlightTaskAutoLine::_generateIdleSetpoints() +{ + // Send zero thrust setpoint */ + _position_setpoint = Vector3f(NAN, NAN, NAN); // Don't require any position/velocity setpoints + _velocity_setpoint = Vector3f(NAN, NAN, NAN); + _thrust_setpoint.zero(); +} + +void FlightTaskAutoLine::_generateLandSetpoints() +{ + // Keep xy-position and go down with landspeed. */ + _position_setpoint = Vector3f(_target(0), _target(1), NAN); + _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _land_speed.get())); +} + +void FlightTaskAutoLine::_generateTakeoffSetpoints() +{ + // Takeoff is completely defined by target position. */ + _position_setpoint = _target; + _velocity_setpoint = Vector3f(NAN, NAN, NAN); +} + +void FlightTaskAutoLine::_generateVelocitySetpoints() +{ + // TODO: Remove velocity force logic from navigator, since + // navigator should only send out waypoints. + _position_setpoint = Vector3f(NAN, NAN, _position(2)); + Vector2f vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed; + _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN); +} + +void FlightTaskAutoLine::_generateSetpoints() +{ + _updateInternalWaypoints(); + _generateAltitudeSetpoints(); + _generateXYsetpoints(); +} + +void FlightTaskAutoLine::_updateInternalWaypoints() +{ + // The internal Waypoints might differ from previous_wp and target. The cases where it differs: + // 1. The vehicle already passed the target -> go straight to target + // 2. The vehicle is more than cruise speed in front of previous waypoint -> go straight to previous waypoint + // 3. The vehicle is more than cruise speed from track -> go straight to closest point on track + // + // If a new target is available, then the speed at the target is computed from the angle previous-target-next. + + // 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 = _getVelocityFromAngle(angle); + } + } + + } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) { + + // Current position is 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 = _getVelocityFromAngle(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 = _getVelocityFromAngle(angle); + } + + } + + } else { + + if ((_target - _destination).length() > 0.01f) { + // A new target is available. Update speed at target.*/ + _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 = _getVelocityFromAngle(angle); + } + } + } +} + +void FlightTaskAutoLine::_generateXYsetpoints() +{ + Vector2f pos_sp_to_dest = Vector2f(&(_target - _position_setpoint)(0)); + 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 and no passing required. Lock position */ + _position_setpoint(0) = _destination(0); + _position_setpoint(1) = _destination(1); + _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f; + + } else { + + // Get various path specific vectors. */ + 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(Vector2f(&_velocity_setpoint(0)) * 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 cannot be more than distance from previous to target + 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) { + // If vehicle wants to stop at the target, then set acceptance radius to zero as well. + acceptance_radius = 0.0f; + } + + if ((target_threshold - acceptance_radius) >= SIGMA_NORM) { + + // Slow down depending on distance to target minus acceptance radius. + 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 = 0.0f; + + if (PX4_ISFINITE(_yaw_wp)) { + 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) { + // accelerate towards target + speed_sp_track = acc_max * _deltatime + speed_sp_prev_track; + } + } + + speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed); + + _position_setpoint(0) = closest_pt(0); + _position_setpoint(1) = closest_pt(1); + Vector2f velocity_sp_xy = u_prev_to_dest * speed_sp_track; + _velocity_setpoint(0) = velocity_sp_xy(0); + _velocity_setpoint(1) = velocity_sp_xy(1); + } +} + +void FlightTaskAutoLine::_generateAltitudeSetpoints() +{ + // Total distance between previous and target set-point. + const float dist = fabsf(_destination(2) - _origin(2)); + + // If target has not been reached, then compute set-point 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 + // upwards with 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(_velocity_setpoint(2))) / _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(_velocity_setpoint(2)); + } + } + + // 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 + _velocity_setpoint(2) = (flying_upward) ? -speed_sp : speed_sp; + _position_setpoint(2) = NAN; // We don't care about position setpoint + + } else { + + // vehicle reached desired target altitude + _velocity_setpoint(2) = 0.0f; + _position_setpoint(2) = _target(2); + } +} + +float FlightTaskAutoLine::_getVelocityFromAngle(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 000000000000..575188d3b02c --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 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() = default; + virtual ~FlightTaskAutoLine() = default; + bool activate() override; + bool update() override; + +protected: + + 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}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, + (ParamFloat) _land_speed, + (ParamFloat) _vel_max_up, + (ParamFloat) _vel_max_down, + (ParamFloat) _acc_max_xy, + (ParamFloat) _acc_xy, + (ParamFloat) _acc_max_up, + (ParamFloat) _acc_max_down, + (ParamFloat) _cruise_speed_90, + (ParamFloat) _nav_rad, + (ParamFloat) _mis_yaw_error + ) + + void _generateIdleSetpoints(); + void _generateLandSetpoints(); + void _generateVelocitySetpoints(); + void _generateTakeoffSetpoints(); + void _updateInternalWaypoints(); /* Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ + void _generateSetpoints(); /**< Generate velocity and position setpoint for following line. */ + void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */ + void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */ + +private: + float _getVelocityFromAngle(const float angle); /** Computes the speed at target depending on angle. */ + void _reset(); /** Resets member variables to current vehicle state */ + WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */ + +}; diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp index 8d3ca33af4a4..a5ba3d276cc3 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp @@ -122,7 +122,7 @@ float FlightTaskManualStabilized::_throttleCurve() float throttle = -((_sticks(2) - 1.0f) * 0.5f); if (throttle < 0.5f) { - return (_throttle_hover.get() - _throttle_min.get()) / 0.5f * throttle + _throttle_min.get(); + return (_throttle_hover.get() - _throttle_min_stabilized.get()) / 0.5f * throttle + _throttle_min_stabilized.get(); } else { return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle - 1.0f) + _throttle_max.get(); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp index df85abd23296..b56f5a141350 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp @@ -69,7 +69,7 @@ class FlightTaskManualStabilized : public FlightTaskManual DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual, (ParamFloat) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */ (ParamFloat) _tilt_max_man, /**< maximum tilt allowed for manual flight */ - (ParamFloat) _throttle_min, /**< minimum throttle that always has to be satisfied in flight*/ + (ParamFloat) _throttle_min_stabilized, /**< minimum throttle for stabilized */ (ParamFloat) _throttle_max, /**< maximum throttle that always has to be satisfied in flight*/ (ParamFloat) _throttle_hover /**< throttle value at which vehicle is at hover equilibrium */ ) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2d3136172981..3b2522c47e94 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2754,6 +2754,7 @@ class MavlinkStreamLocalPositionSetpoint : public MavlinkStream msg.y = pos_sp.y; msg.z = pos_sp.z; msg.yaw = pos_sp.yaw; + msg.yaw_rate = pos_sp.yawspeed; msg.vx = pos_sp.vx; msg.vy = pos_sp.vy; msg.vz = pos_sp.vz; diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index 3dec5711d4a0..d002ccae7238 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * 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 @@ -33,43 +33,18 @@ /** * @file PositionControl.cpp - * - * This file implements a P-position-control cascaded with a - * PID-velocity-controller. - * - * Inputs: vehicle states (pos, vel, q) - * desired setpoints (pos, vel, thrust, yaw) - * Outputs: thrust and yaw setpoint */ #include "PositionControl.hpp" #include #include -#include "uORB/topics/parameter_update.h" #include "Utility/ControlMath.hpp" using namespace matrix; -PositionControl::PositionControl() -{ - _Pz_h = param_find("MPC_Z_P"); - _Pvz_h = param_find("MPC_Z_VEL_P"); - _Ivz_h = param_find("MPC_Z_VEL_I"); - _Dvz_h = param_find("MPC_Z_VEL_D"); - _Pxy_h = param_find("MPC_XY_P"); - _Pvxy_h = param_find("MPC_XY_VEL_P"); - _Ivxy_h = param_find("MPC_XY_VEL_I"); - _Dvxy_h = param_find("MPC_XY_VEL_D"); - _VelMaxXY_h = param_find("MPC_XY_VEL_MAX"); - _VelMaxZdown_h = param_find("MPC_Z_VEL_MAX_DN"); - _VelMaxZup_h = param_find("MPC_Z_VEL_MAX_UP"); - _ThrHover_h = param_find("MPC_THR_HOVER"); - _ThrMax_h = param_find("MPC_THR_MAX"); - _ThrMin_h = param_find("MPC_THR_MIN"); - - /* Set parameter the very first time. */ - _setParams(); -}; +PositionControl::PositionControl(ModuleParams *parent) : + ModuleParams(parent) +{} void PositionControl::updateState(const struct vehicle_local_position_s state, const Vector3f &vel_dot) { @@ -89,24 +64,35 @@ void PositionControl::updateSetpoint(struct vehicle_local_position_setpoint_s se _yawspeed_sp = setpoint.yawspeed; _interfaceMapping(); - /* If full manual is required (thrust already generated), don't run position/velocity - * controller and just return thrust. - */ - _skipController = false; + // If full manual is required (thrust already generated), don't run position/velocity + // controller and just return thrust. + _skip_controller = false; if (PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]) && PX4_ISFINITE(setpoint.thrust[2])) { - _skipController = true; + _skip_controller = true; } } void PositionControl::generateThrustYawSetpoint(const float &dt) { - _updateParams(); + if (_skip_controller) { + // Already received a valid thrust set-point. + // Limit the thrust vector. + float thr_mag = _thr_sp.length(); + + if (thr_mag > MPC_THR_MAX.get()) { + _thr_sp = _thr_sp.normalized() * MPC_THR_MAX.get(); - /* Only run position/velocity controller - * if thrust needs to be generated - */ - if (!_skipController) { + } else if (thr_mag < MPC_THR_MIN.get() && thr_mag > FLT_EPSILON) { + _thr_sp = _thr_sp.normalized() * MPC_THR_MIN.get(); + } + + // Just set the set-points equal to the current vehicle state. + _pos_sp = _pos; + _vel_sp = _vel; + _acc_sp = _acc; + + } else { _positionController(); _velocityController(dt); } @@ -114,44 +100,40 @@ void PositionControl::generateThrustYawSetpoint(const float &dt) void PositionControl::_interfaceMapping() { - /* Respects FlightTask interface, where - * NAN-setpoints are of no interest and - * do not require control. - */ - - /* Loop through x,y and z components of all setpoints. */ + // Respects FlightTask interface, where NAN-set-points are of no interest + // and do not require control. A valide position and velocity setpoint will + // be mapped to a desired position setpoint with a feed-forward term. for (int i = 0; i <= 2; i++) { if (PX4_ISFINITE(_pos_sp(i))) { - - /* Position control is required */ + // Position control is required if (!PX4_ISFINITE(_vel_sp(i))) { - /* Velocity is not used as feedforward term. */ + // Velocity is not used as feedforward term. _vel_sp(i) = 0.0f; } - /* thrust setpoint is not supported - * in position control - */ + // thrust setpoint is not supported in position control _thr_sp(i) = 0.0f; } else if (PX4_ISFINITE(_vel_sp(i))) { - /* Velocity controller is active without - * position control. - */ + // Velocity controller is active without position control. + // Set the desired position set-point equal to current position + // such that error is zero. _pos_sp(i) = _pos(i); + // thrust setpoint is not supported in position control _thr_sp(i) = 0.0f; } else if (PX4_ISFINITE(_thr_sp(i))) { - /* Thrust setpoint was generated from - * stick directly. - */ + // Thrust setpoint was generated from sticks directly. + // Set all other set-points equal MC states. _pos_sp(i) = _pos(i); _vel_sp(i) = _vel(i); + // Reset the Integral term. _thr_int(i) = 0.0f; + // Don't require velocity derivative. _vel_dot(i) = 0.0f; } else { @@ -160,127 +142,114 @@ void PositionControl::_interfaceMapping() } if (!PX4_ISFINITE(_yawspeed_sp)) { + // Set the yawspeed to 0 since not used. _yawspeed_sp = 0.0f; } if (!PX4_ISFINITE(_yaw_sp)) { + // Set the yaw-sp equal the current yaw. + // That is the best we can do and it also + // agrees with FlightTask-interface definition. _yaw_sp = _yaw; } } void PositionControl::_positionController() { - /* Generate desired velocity setpoint */ - - /* P-controller */ - _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)); - - 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; - } - - /* Saturate velocity in D-direction */ - _vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ.up, _VelMaxZ.down); + // P-position controller + Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(MPC_XY_P.get(), MPC_XY_P.get(), MPC_Z_P.get())); + _vel_sp = vel_sp_position + _vel_sp; + + // Constrain horizontal velocity by prioritizing the velocity component along the + // the desired position setpoint over the feed-forward term. + Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(&vel_sp_position(0)), + Vector2f(&(_vel_sp - vel_sp_position)(0)), MPC_XY_VEL_MAX.get()); + _vel_sp(0) = vel_sp_xy(0); + _vel_sp(1) = vel_sp_xy(1); + // Constrain velocity in z-direction. + _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.vel_max_z_up, MPC_Z_VEL_MAX_DN.get()); } void PositionControl::_velocityController(const float &dt) { - /* Generate desired thrust setpoint */ - /* PID - * u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral) - * Umin <= u_des <= Umax - * - * Anti-Windup: - * u_des = _thr_sp; r = _vel_sp; y = _vel - * u_des >= Umax and r - y >= 0 => Saturation = true - * u_des >= Umax and r - y <= 0 => Saturation = false - * u_des <= Umin and r - y <= 0 => Saturation = true - * u_des <= Umin and r - y >= 0 => Saturation = false - * - * Notes: - * - PID implementation is in NED-frame - * - control output in D-direction has priority over NE-direction - * - the equilibrium point for the PID is at hover-thrust - * - the maximum tilt cannot exceed 90 degrees. This means that it is - * not possible to have a desired thrust direction pointing in the positive - * D-direction (= downward) - * - the desired thrust in D-direction is limited by the thrust limits - * - the desired thrust in NE-direction is limited by the thrust excess after - * consideration of the desired thrust in D-direction. In addition, the thrust in - * NE-direction is also limited by the maximum tilt. - */ - - /* Get maximum tilt */ - float tilt_max = M_PI_2_F; - - if (PX4_ISFINITE(_constraints.tilt_max) && _constraints.tilt_max <= tilt_max) { - tilt_max = _constraints.tilt_max; - } - Vector3f vel_err = _vel_sp - _vel; + // Generate desired thrust setpoint. + // PID + // u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral) + // Umin <= u_des <= Umax + // + // Anti-Windup: + // u_des = _thr_sp; r = _vel_sp; y = _vel + // u_des >= Umax and r - y >= 0 => Saturation = true + // u_des >= Umax and r - y <= 0 => Saturation = false + // u_des <= Umin and r - y <= 0 => Saturation = true + // u_des <= Umin and r - y >= 0 => Saturation = false + // + // Notes: + // - PID implementation is in NED-frame + // - control output in D-direction has priority over NE-direction + // - the equilibrium point for the PID is at hover-thrust + // - the maximum tilt cannot exceed 90 degrees. This means that it is + // not possible to have a desired thrust direction pointing in the positive + // D-direction (= downward) + // - the desired thrust in D-direction is limited by the thrust limits + // - the desired thrust in NE-direction is limited by the thrust excess after + // consideration of the desired thrust in D-direction. In addition, the thrust in + // NE-direction is also limited by the maximum tilt. - /* - * TODO: add offboard acceleration mode - * */ + Vector3f vel_err = _vel_sp - _vel; - /* Consider thrust in D-direction */ - float thrust_desired_D = Pv(2) * vel_err(2) + Dv(2) * _vel_dot(2) + _thr_int(2) - _ThrHover; + // Consider thrust in D-direction. + float thrust_desired_D = MPC_Z_VEL_P.get() * vel_err(2) + MPC_Z_VEL_D.get() * _vel_dot(2) + _thr_int( + 2) - MPC_THR_HOVER.get(); - /* The Thrust limits are negated and swapped due to NED-frame */ - float uMax = -_ThrustLimit.min; - float uMin = -_ThrustLimit.max; + // The Thrust limits are negated and swapped due to NED-frame. + float uMax = -MPC_THR_MIN.get(); + float uMin = -MPC_THR_MAX.get(); - /* Apply Anti-Windup in D-direction */ + // Apply Anti-Windup in D-direction. bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) || (thrust_desired_D <= uMin && vel_err(2) <= 0.0f); if (!stop_integral_D) { - _thr_int(2) += vel_err(2) * Iv(2) * dt; + _thr_int(2) += vel_err(2) * MPC_Z_VEL_I.get() * dt; } - /* Saturate thrust setpoint in D-direction */ + // Saturate thrust setpoint in D-direction. _thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax); if (fabsf(_thr_sp(0)) + fabsf(_thr_sp(1)) > FLT_EPSILON) { - - /* Thrust setpoints in NE-direction is already provided. Only - * scaling is required. - */ - - float thr_xy_max = fabsf(_thr_sp(2)) * tanf(tilt_max); + // Thrust set-point in NE-direction is already provided. Only + // scaling by the maximum tilt is required. + float thr_xy_max = fabsf(_thr_sp(2)) * tanf(MPC_MAN_TILT_MAX_rad.get()); _thr_sp(0) *= thr_xy_max; _thr_sp(1) *= thr_xy_max; } else { - - /* PID for NE-direction */ + // PID-velocity controller for NE-direction. Vector2f thrust_desired_NE; - thrust_desired_NE(0) = Pv(0) * vel_err(0) + Dv(0) * _vel_dot(0) + _thr_int(0); - thrust_desired_NE(1) = Pv(1) * vel_err(1) + Dv(1) * _vel_dot(1) + _thr_int(1); + thrust_desired_NE(0) = MPC_XY_VEL_P.get() * vel_err(0) + MPC_XY_VEL_D.get() * _vel_dot(0) + _thr_int(0); + thrust_desired_NE(1) = MPC_XY_VEL_P.get() * vel_err(1) + MPC_XY_VEL_D.get() * _vel_dot(1) + _thr_int(1); - /* Get maximum allowed thrust in NE based on tilt and excess thrust */ - float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(tilt_max); - float thrust_max_NE = sqrtf(_ThrustLimit.max * _ThrustLimit.max - _thr_sp(2) * _thr_sp(2)); + // Get maximum allowed thrust in NE based on tilt and excess thrust. + float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt_max); + float thrust_max_NE = sqrtf(MPC_THR_MAX.get() * MPC_THR_MAX.get() - _thr_sp(2) * _thr_sp(2)); thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE); - /* Get the direction of (r-y) in NE-direction */ + // Get the direction of (r-y) in NE-direction. float direction_NE = Vector2f(&vel_err(0)) * Vector2f(&_vel_sp(0)); - /* Apply Anti-Windup in NE-direction */ + // Apply Anti-Windup in NE-direction. bool stop_integral_NE = (thrust_desired_NE * thrust_desired_NE >= thrust_max_NE * thrust_max_NE && direction_NE >= 0.0f); if (!stop_integral_NE) { - _thr_int(0) += vel_err(0) * Iv(0) * dt; - _thr_int(1) += vel_err(1) * Iv(1) * dt; + _thr_int(0) += vel_err(0) * MPC_XY_VEL_I.get() * dt; + _thr_int(1) += vel_err(1) * MPC_XY_VEL_I.get() * dt; } - /* Saturate thrust in NE-direction */ + // Saturate thrust in NE-direction. _thr_sp(0) = thrust_desired_NE(0); _thr_sp(1) = thrust_desired_NE(1); @@ -291,49 +260,29 @@ void PositionControl::_velocityController(const float &dt) } } - } void PositionControl::updateConstraints(const Controller::Constraints &constraints) { _constraints = constraints; -} -void PositionControl::_updateParams() -{ - bool updated; - parameter_update_s param_update; - orb_check(_parameter_sub, &updated); + // Check if adjustable constraints are below global constraints. If they are not stricter than global + // constraints, then just use global constraints for the limits. + + if (!PX4_ISFINITE(constraints.tilt_max) || !(constraints.tilt_max < MPC_TILTMAX_AIR_rad.get())) { + _constraints.tilt_max = MPC_TILTMAX_AIR_rad.get(); + } - if (updated) { - orb_copy(ORB_ID(parameter_update), _parameter_sub, ¶m_update); - _setParams(); + if (!PX4_ISFINITE(constraints.vel_max_z_up) || !(constraints.vel_max_z_up < MPC_Z_VEL_MAX_UP.get())) { + _constraints.vel_max_z_up = MPC_Z_VEL_MAX_UP.get(); } } -void PositionControl::_setParams() +void PositionControl::updateParams() { - param_get(_Pxy_h, &Pp(0)); - param_get(_Pxy_h, &Pp(1)); - param_get(_Pz_h, &Pp(2)); - - param_get(_Pvxy_h, &Pv(0)); - param_get(_Pvxy_h, &Pv(1)); - param_get(_Pvz_h, &Pv(2)); - - param_get(_Ivxy_h, &Iv(0)); - param_get(_Ivxy_h, &Iv(1)); - param_get(_Ivz_h, &Iv(2)); - - param_get(_Dvxy_h, &Dv(0)); - param_get(_Dvxy_h, &Dv(1)); - param_get(_Dvz_h, &Dv(2)); - - param_get(_VelMaxXY_h, &_VelMaxXY); - param_get(_VelMaxZup_h, &_VelMaxZ.up); - param_get(_VelMaxZdown_h, &_VelMaxZ.down); + ModuleParams::updateParams(); - param_get(_ThrHover_h, &_ThrHover); - param_get(_ThrMax_h, &_ThrustLimit.max); - param_get(_ThrMin_h, &_ThrustLimit.min); + // Tilt needs to be in radians + MPC_TILTMAX_AIR_rad.set(math::radians(MPC_TILTMAX_AIR_rad.get())); + MPC_MAN_TILT_MAX_rad.set(math::radians(MPC_MAN_TILT_MAX_rad.get())); } diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp index c694d91b3ea1..0d6a0ed85859 100644 --- a/src/modules/mc_pos_control/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * 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 @@ -34,105 +34,182 @@ /** * @file PositionControl.hpp * - * @inputs: position-, velocity-, acceleration-, thrust-setpoints - * @outputs: thrust vector - * + * A cascaded position controller for position/velocity control only. */ #include #include #include -#include +#include #pragma once -/* Constraints based on mode: - * Eventually this structure should be part of local position message - */ namespace Controller { +/** Constraints that depends on mode and are lower + * than the global limits. + * tilt_max: Cannot exceed PI/2 radians + * vel_max_z_up: Cannot exceed maximum global velocity upwards + * @see MPC_TILTMAX_AIR + * @see MPC_Z_VEL_MAX_DN + */ struct Constraints { - float tilt_max; + float tilt_max; /**< maximum tilt always below Pi/2 */ + float vel_max_z_up; /**< maximum speed upwards always smaller than MPC_VEL_Z_MAX_UP */ }; } - -class PositionControl +/** + * Core Position-Control for MC. + * This class contains P-controller for position and + * PID-controller for velocity. + * Inputs: + * vehicle position/velocity/yaw + * desired set-point position/velocity/thrust/yaw/yaw-speed + * constraints that are stricter than global limits + * Output + * thrust vector and a yaw-setpoint + * + * If there is a position and a velocity set-point present, then + * the velocity set-point is used as feed-forward. If feed-forward is + * active, then the velocity component of the P-controller output has + * priority over the feed-forward component. + * + * A setpoint that is NAN is considered as not set. + */ +class PositionControl : public ModuleParams { public: - PositionControl(); - - ~PositionControl() {}; + PositionControl(ModuleParams *parent); + ~PositionControl() = default; + + /** + * Overwrites certain parameters. + * Overwrites are required for unit-conversion. + * This method should only be called if parameters + * have been updated. + */ + void overwriteParams(); + + /** + * Update the current vehicle state. + * @param state a vehicle_local_position_s structure + * @param vel_dot the derivative of the vehicle velocity + */ void updateState(const struct vehicle_local_position_s state, const matrix::Vector3f &vel_dot); + + /** + * Update the desired setpoints. + * @param setpoint a vehicle_local_position_setpoint_s structure + */ void updateSetpoint(struct vehicle_local_position_setpoint_s setpoint); + + /** + * Set constraints that are stricter than the global limits. + * @param constraints a PositionControl structure with supported constraints + */ void updateConstraints(const Controller::Constraints &constraints); + + /** + * Apply P-position and PID-velocity controller that updates the member + * thrust, yaw- and yawspeed-setpoints. + * @see _thr_sp + * @see _yaw_sp + * @see _yawspeed_sp + * @param dt the delta-time + */ void generateThrustYawSetpoint(const float &dt); + /** + * Set the integral term in xy to 0. + * @see _thr_int + */ + void resetIntegralXY() {_thr_int(0) = _thr_int(1) = 0.0f;}; + + /** + * Set the integral term in z to 0. + * @see _thr_int + */ + void resetIntegralZ() {_thr_int(2) = 0.0f;}; + + /** + * Get the + * @see _thr_sp + * @return The thrust set-point member. + */ matrix::Vector3f getThrustSetpoint() {return _thr_sp;} + + /** + * Get the + * @see _yaw_sp + * @return The yaw set-point member. + */ float getYawSetpoint() { return _yaw_sp;} + + /** + * Get the + * @see _yawspeed_sp + * @return The yawspeed set-point member. + */ float getYawspeedSetpoint() {return _yawspeed_sp;} + + /** + * Get the + * @see _vel_sp + * @return The velocity set-point member. + */ matrix::Vector3f getVelSp() {return _vel_sp;} + + /** + * Get the + * @see _pos_sp + * @return The position set-point member. + */ matrix::Vector3f getPosSp() {return _pos_sp;} -private: +protected: - /* States */ - matrix::Vector3f _pos{}; - matrix::Vector3f _vel{}; - matrix::Vector3f _vel_dot{}; - matrix::Vector3f _acc{}; - float _yaw{0.0f}; - - /* Setpoints */ - matrix::Vector3f _pos_sp{}; - matrix::Vector3f _vel_sp{}; - matrix::Vector3f _acc_sp{}; - matrix::Vector3f _thr_sp{}; - float _yaw_sp{}; - float _yawspeed_sp{}; - - /* Other variables */ - matrix::Vector3f _thr_int{}; - Controller::Constraints _constraints{}; - - /* Parameter handles */ - int _parameter_sub{-1}; - param_t _Pz_h{PARAM_INVALID}; - param_t _Pvz_h{PARAM_INVALID}; - param_t _Ivz_h{PARAM_INVALID}; - param_t _Dvz_h{PARAM_INVALID}; - param_t _Pxy_h{PARAM_INVALID}; - param_t _Pvxy_h{PARAM_INVALID}; - param_t _Ivxy_h{PARAM_INVALID}; - param_t _Dvxy_h{PARAM_INVALID}; - param_t _VelMaxXY_h{PARAM_INVALID}; - param_t _VelMaxZdown_h{PARAM_INVALID}; - param_t _VelMaxZup_h{PARAM_INVALID}; - param_t _ThrHover_h{PARAM_INVALID}; - param_t _ThrMax_h{PARAM_INVALID}; - param_t _ThrMin_h{PARAM_INVALID}; - - /* Parameters */ - matrix::Vector3f Pp, Pv, Iv, Dv = matrix::Vector3f{0.0f, 0.0f, 0.0f}; - float _VelMaxXY{}; - struct DirectionD { - float up; - float down; - }; - DirectionD _VelMaxZ; - struct Limits { - float max; - float min; - }; - Limits _ThrustLimit; - float _ThrHover{0.5f}; - bool _skipController{false}; - - /* Helper methods */ - void _interfaceMapping(); - void _positionController(); - void _velocityController(const float &dt); - void _updateParams(); - void _setParams(); + void updateParams() override; + +private: + void _interfaceMapping(); /** maps set-points to internal member set-points */ + void _positionController(); /** applies the P-position-controller */ + void _velocityController(const float &dt); /** applies the PID-velocity-controller */ + + matrix::Vector3f _pos{}; /**< MC position */ + matrix::Vector3f _vel{}; /**< MC velocity */ + matrix::Vector3f _vel_dot{}; /**< MC velocity derivative */ + matrix::Vector3f _acc{}; /**< MC acceleration */ + float _yaw{0.0f}; /**< MC yaw */ + matrix::Vector3f _pos_sp{}; /**< desired position */ + matrix::Vector3f _vel_sp{}; /**< desired velocity */ + matrix::Vector3f _acc_sp{}; /**< desired acceleration: not supported yet */ + matrix::Vector3f _thr_sp{}; /**< desired thrust */ + float _yaw_sp{}; /**< desired yaw */ + float _yawspeed_sp{}; /** desired yaw-speed */ + matrix::Vector3f _thr_int{}; /**< thrust integral term */ + Controller::Constraints _constraints{}; /**< variable constraints */ + bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */ + + DEFINE_PARAMETERS( + (ParamFloat) MPC_THR_MAX, + (ParamFloat) MPC_THR_HOVER, + (ParamFloat) MPC_THR_MIN, + (ParamFloat) MPC_MANTHR_MIN, + (ParamFloat) MPC_XY_VEL_MAX, + (ParamFloat) MPC_Z_VEL_MAX_DN, + (ParamFloat) MPC_Z_VEL_MAX_UP, + (ParamFloat) + MPC_TILTMAX_AIR_rad, // maximum tilt for any position controlled mode in radians + (ParamFloat) MPC_MAN_TILT_MAX_rad, // maximum til for stabilized/altitude mode in radians + (ParamFloat) MPC_Z_P, + (ParamFloat) MPC_Z_VEL_P, + (ParamFloat) MPC_Z_VEL_I, + (ParamFloat) MPC_Z_VEL_D, + (ParamFloat) MPC_XY_P, + (ParamFloat) MPC_XY_VEL_P, + (ParamFloat) MPC_XY_VEL_I, + (ParamFloat) MPC_XY_VEL_D + ) }; diff --git a/src/modules/mc_pos_control/Utility/ControlMath.cpp b/src/modules/mc_pos_control/Utility/ControlMath.cpp index 9235907b7c56..73e937cd1fff 100644 --- a/src/modules/mc_pos_control/Utility/ControlMath.cpp +++ b/src/modules/mc_pos_control/Utility/ControlMath.cpp @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * 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 @@ -34,12 +34,8 @@ /** * @file ControlMath.cpp - * - * Simple functions for vector manipulation that do not fit into matrix lib. - * These functions are specific for controls. */ - #include "ControlMath.hpp" #include #include @@ -49,30 +45,29 @@ namespace ControlMath { vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp) { - vehicle_attitude_setpoint_s att_sp; att_sp.yaw_body = yaw_sp; - /* desired body_z axis = -normalize(thrust_vector) */ + // desired body_z axis = -normalize(thrust_vector) matrix::Vector3f body_x, body_y, body_z; if (thr_sp.length() > 0.00001f) { body_z = -thr_sp.normalized(); } else { - /* no thrust, set Z axis to safe value */ + // no thrust, set Z axis to safe value body_z.zero(); body_z(2) = 1.0f; } - /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + // vector of desired yaw direction in XY plane, rotated by PI/2 matrix::Vector3f y_C(-sinf(att_sp.yaw_body), cosf(att_sp.yaw_body), 0.0f); if (fabsf(body_z(2)) > 0.000001f) { - /* desired body_x axis, orthogonal to body_z */ + // desired body_x axis, orthogonal to body_z body_x = y_C % body_z; - /* keep nose to front while inverted upside down */ + // keep nose to front while inverted upside down if (body_z(2) < 0.0f) { body_x = -body_x; } @@ -80,37 +75,101 @@ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, con body_x.normalize(); } else { - /* desired thrust is in XY plane, set X downside to construct correct matrix, - * but yaw component will not be used actually */ + // desired thrust is in XY plane, set X downside to construct correct matrix, + // but yaw component will not be used actually body_x.zero(); body_x(2) = 1.0f; } - /* desired body_y axis */ + // desired body_y axis body_y = body_z % body_x; matrix::Dcmf R_sp; - /* fill rotation matrix */ + // fill rotation matrix for (int i = 0; i < 3; i++) { R_sp(i, 0) = body_x(i); R_sp(i, 1) = body_y(i); R_sp(i, 2) = body_z(i); } - /* copy quaternion setpoint to attitude setpoint topic */ + //copy quaternion setpoint to attitude setpoint topic matrix::Quatf q_sp = R_sp; q_sp.copyTo(att_sp.q_d); att_sp.q_d_valid = true; - /* calculate euler angles, for logging only, must not be used for control */ + // calculate euler angles, for logging only, must not be used for control matrix::Eulerf euler = R_sp; att_sp.roll_body = euler(0); att_sp.pitch_body = euler(1); - /* fill and publish att_sp message */ + // fill and publish att_sp message att_sp.thrust = thr_sp.length(); return att_sp; } +matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f &v1, const float max) +{ + if (matrix::Vector2f(v0 + v1).norm() <= max) { + // vector does not exceed maximum magnitude + return v0 + v1; + + } else if (v0.length() >= max) { + // the magnitude along v0, which has priority, already exceeds maximum. + return v0.normalized() * max; + + } else if (fabsf(matrix::Vector2f(v1 - v0).norm()) < 0.001f) { + // the two vectors are equal + return v0.normalized() * max; + + } else if (v0.length() < 0.001f) { + // the first vector is 0. + return v1.normalized() * max; + + } else { + // vf = final vector with ||vf|| <= max + // s = scaling factor + // u1 = unit of v1 + // vf = v0 + v1 = v0 + s * u1 + // constraint: ||vf|| <= max + // + // solve for s: ||vf|| = ||v0 + s * u1|| <= max + // + // Derivation: + // For simplicity, replace v0 -> v, u1 -> u + // v0(0/1/2) -> v0/1/2 + // u1(0/1/2) -> u0/1/2 + // + // ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v1+s*u1)^2 = max^2 + // v0^2+2*s*u0*v0+s^2*u0^2 + v1^2+2*s*u1*v1+s^2*u1^2 + v2^2+2*s*u2*v2+s^2*u2^2 = max^2 + // s^2*(u0^2+u1^2+u2^2) + s*2*(u0*v0+u1*v1+u2*v2) + (v0^2+v1^2+v2^2-max^2) = 0 + // + // quadratic equation: + // -> s^2*a + s*b + c = 0 with solution: s1/2 = (-b +- sqrt(b^2 - 4*a*c))/(2*a) + // + // b = 2 * u.dot(v) + // a = 1 (because u is normalized) + // c = (v0^2+v1^2+v2^2-max^2) = -max^2 + ||v||^2 + // + // sqrt(b^2 - 4*a*c) = + // sqrt(4*u.dot(v)^2 - 4*(||v||^2 - max^2)) = 2*sqrt(u.dot(v)^2 +- (||v||^2 -max^2)) + // + // s1/2 = ( -2*u.dot(v) +- 2*sqrt(u.dot(v)^2 - (||v||^2 -max^2)) / 2 + // = -u.dot(v) +- sqrt(u.dot(v)^2 - (||v||^2 -max^2)) + // m = u.dot(v) + // s = -m + sqrt(m^2 - c) + // + // + // + // notes: + // - s (=scaling factor) needs to be positive + // - (max - ||v||) always larger than zero, otherwise it never entered this if-statement + + matrix::Vector2f u1 = v1.normalized(); + float m = u1.dot(v0); + float c = v0.length() * v0.length() - max * max; + float s = -m + sqrtf(m * m - c); + return v0 + u1 * s; + } +} } diff --git a/src/modules/mc_pos_control/Utility/ControlMath.hpp b/src/modules/mc_pos_control/Utility/ControlMath.hpp index c82b5285d06a..a685a43a025f 100644 --- a/src/modules/mc_pos_control/Utility/ControlMath.hpp +++ b/src/modules/mc_pos_control/Utility/ControlMath.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * 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 @@ -38,7 +38,6 @@ * These functions are specific for controls. */ - #pragma once #include @@ -46,5 +45,23 @@ namespace ControlMath { +/** + * Converts thrust vector and yaw set-point to a desired attitude. + * @param thr_sp a 3D vector + * @param yaw_sp the desired yaw + * @return vehicle_attitude_setpoints_s structure + */ vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp); + +/** + * Outputs the sum of two vectors but respecting the limits and priority. + * The sum of two vectors are constraint such that v0 has priority over v1. + * This means that if the length of (v0+v1) exceeds max, then it is constraint such + * that v0 has priority. + * + * @param v0 a 2D vector that has priority given the maximum available magnitude. + * @param v1 a 2D vector that less priority given the maximum available magnitude. + * @return 2D vector + */ +matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f &v1, const float max); } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index c03c6507cd08..5002c3739f12 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -238,7 +238,7 @@ class MulticopterPositionControl : public control::SuperBlock, public ModulePara FlightTasks _flight_tasks; /**< class handling all ways to generate position controller setpoints */ - PositionControl _control{}; /**< class handling the core PID position controller */ + PositionControl _control; /**< class handling the core PID position controller */ systemlib::Hysteresis _manual_direction_change_hysteresis; @@ -301,6 +301,7 @@ class MulticopterPositionControl : public control::SuperBlock, public ModulePara float _min_hagl_limit; /**< minimum continuous height above ground (m) */ + float _takeoff_speed; /**< For flighttask interface used only. It can be thrust or velocity setpoints */ // counters for reset events on position and velocity states // they are used to identify a reset event uint8_t _z_reset_counter; @@ -394,6 +395,8 @@ class MulticopterPositionControl : public control::SuperBlock, public ModulePara bool manual_wants_takeoff(); + void update_smooth_takeoff(); + void set_takeoff_velocity(float &vel_sp_z); void landdetection_thrust_limit(matrix::Vector3f &thrust_sp); @@ -403,7 +406,7 @@ class MulticopterPositionControl : public control::SuperBlock, public ModulePara /** * Temporary method for flight control compuation */ - void updateConstraints(Controller::Constraints &constrains); + void updateTiltConstraints(Controller::Constraints &constrains); void publish_attitude(); @@ -458,6 +461,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_x_deriv(this, "VELD"), _vel_y_deriv(this, "VELD"), _vel_z_deriv(this, "VELD"), + _control(this), _manual_direction_change_hysteresis(false), _filter_manual_pitch(50.0f, 10.0f), _filter_manual_roll(50.0f, 10.0f), @@ -632,6 +636,7 @@ MulticopterPositionControl::parameters_update(bool force) /* we only use jerk for braking if jerk_hor_max > jerk_hor_min; otherwise just set jerk very large */ _manual_jerk_limit_z = (_jerk_hor_max.get() > _jerk_hor_min.get()) ? _jerk_hor_max.get() : 1000000.f; + /* Get parameter values used to fly within optical flow sensor limits */ param_t handle = param_find("SENS_FLOW_MINRNG"); @@ -2130,7 +2135,7 @@ void MulticopterPositionControl::control_auto() } } else { - /* we are more than cruise_speed away from track */ + /* We are more than cruise speed away from track */ /* if previous is in front just go directly to previous point */ if (previous_in_front) { @@ -3020,19 +3025,6 @@ MulticopterPositionControl::task_main() _vel_sp_prev = _vel; } - if (!_in_smooth_takeoff && _vehicle_land_detected.landed && _control_mode.flag_armed && - (in_auto_takeoff() || manual_wants_takeoff())) { - _in_smooth_takeoff = true; - // This ramp starts negative and goes to positive later because we want to - // be as smooth as possible. If we start at 0, we alrady jump to hover throttle. - _takeoff_vel_limit = -0.5f; - } - - else if (!_control_mode.flag_armed) { - // If we're disarmed and for some reason were in a smooth takeoff, we reset that. - _in_smooth_takeoff = false; - } - /* set triplets to invalid if we just landed */ if (_vehicle_land_detected.landed && !was_landed) { _pos_sp_triplet.current.valid = false; @@ -3068,6 +3060,16 @@ MulticopterPositionControl::task_main() _flight_tasks.switchTask(FlightTaskIndex::Stabilized); break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + + /*TODO: clean up navigation state and commander state, which both share too many equal states */ + _flight_tasks.switchTask(FlightTaskIndex::AutoLine); + break; + default: /* not supported yet */ _flight_tasks.switchTask(FlightTaskIndex::None); @@ -3081,77 +3083,131 @@ MulticopterPositionControl::task_main() if (_test_flight_tasks.get() && _flight_tasks.isAnyTaskActive()) { _flight_tasks.update(); - - /* Get Flighttask setpoints */ vehicle_local_position_setpoint_s setpoint = _flight_tasks.getPositionSetpoint(); - /* Get _contstraints depending on flight mode - * This logic will be set by FlightTasks */ + // structure that replaces global constraints such as tilt, + // maximum velocity in z-direction ... Controller::Constraints constraints; - updateConstraints(constraints); + constraints.vel_max_z_up = NAN; // NAN for not used + + // Check for smooth takeoff + if (_vehicle_land_detected.landed && !_in_smooth_takeoff && _control_mode.flag_armed) { + // Vehicle is still landed and no takeoff was initiated yet. + // Adjust for different takeoff cases. + // The minimum takeoff altitude needs to be at least 20cm above current position + if ((PX4_ISFINITE(setpoint.z) && setpoint.z < _pos(2) - 0.2f) || + (PX4_ISFINITE(setpoint.vz) && setpoint.vz < math::min(-_tko_speed.get(), -0.6f))) { + // There is a position setpoint above current position or velocity setpoint larger than + // takeoff speed. Enable smooth takeoff. + _in_smooth_takeoff = true; + _takeoff_speed = -0.5f; - /* For takeoff we adjust the velocity setpoint in the z-direction */ - if (_in_smooth_takeoff) { - /* Adjust velocity setpoint in z if we are in smooth takeoff */ - set_takeoff_velocity(setpoint.vz); + } else { + // Default + _in_smooth_takeoff = false; + } } - /* this logic is only temporary. - * Mode switch related things will be handled within - * Flighttask activate method - */ - if (_vehicle_status.nav_state - == _vehicle_status.NAVIGATION_STATE_MANUAL) { - /* we set triplets to false - * this ensures that when switching to auto, the position - * controller will not use the old triplets but waits until triplets - * have been updated */ - _mode_auto = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.previous.valid = false; - _hold_offboard_xy = false; - _hold_offboard_z = false; + // If in smooth takeoff, adjust setpoints based on what is valid: + // 1. position setpoint is valid -> go with takeoffspeed to specific altitude + // 2. position setpoint not valid but velocity setpoint valid: ramp up velocity + if (_in_smooth_takeoff) { + float desired_tko_speed = -setpoint.vz; + + // If there is a valid position setpoint, then set the desired speed to the takeoff speed. + if (PX4_ISFINITE(setpoint.z)) { + desired_tko_speed = _tko_speed.get(); + } + + // Ramp up takeoff speed. + _takeoff_speed += desired_tko_speed * _dt / _takeoff_ramp_time.get(); + _takeoff_speed = math::min(_takeoff_speed, desired_tko_speed); + // Limit the velocity setpoint from the position controller + constraints.vel_max_z_up = _takeoff_speed; + + } else { + _in_smooth_takeoff = false; } - // We can only run the control if we're already in-air, have a takeoff setpoint, - // or if we're in offboard control. - // Otherwise, we should just bail out - if (_vehicle_land_detected.landed && !in_auto_takeoff() && !manual_wants_takeoff()) { - // Keep throttle low while still on ground. - set_idle_state(); + // We can only run the control if we're already in-air, have a takeoff setpoint, and are not + // in pure manual. Otherwise just stay idle. + if (_vehicle_land_detected.landed && !_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) { + // Keep throttle low + setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; + setpoint.yawspeed = 0.0f; + setpoint.yaw = _yaw; + } - } else if (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_MANUAL || - _vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_POSCTL || - _vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_ALTCTL) { + // Update tilt constraints. For now it still requires to know about control mode + // TODO: check if it makes sense to have a tilt constraint for landing. + updateTiltConstraints(constraints); + // Update states, setpoints and constraints. + _control.updateConstraints(constraints); + _control.updateState(_local_pos, matrix::Vector3f(&(_vel_err_d(0)))); + _control.updateSetpoint(setpoint); - _control.updateState(_local_pos, matrix::Vector3f(&(_vel_err_d(0)))); - _control.updateSetpoint(setpoint); - _control.updateConstraints(constraints); - _control.generateThrustYawSetpoint(_dt); + // Generate desired thrust and yaw. + _control.generateThrustYawSetpoint(_dt); + matrix::Vector3f thr_sp = _control.getThrustSetpoint(); - /* fill local position, velocity and thrust setpoint */ - _local_pos_sp.timestamp = hrt_absolute_time(); - _local_pos_sp.x = _control.getPosSp()(0); - _local_pos_sp.y = _control.getPosSp()(1); - _local_pos_sp.z = _control.getPosSp()(2); - _local_pos_sp.yaw = _control.getYawSetpoint(); - _local_pos_sp.yawspeed = _control.getYawspeedSetpoint(); - _local_pos_sp.vx = _control.getVelSp()(0); - _local_pos_sp.vy = _control.getVelSp()(1); - _local_pos_sp.vz = _control.getVelSp()(2); - _control.getThrustSetpoint().copyTo(_local_pos_sp.thrust); - - /* We adjust thrust setpoint based on landdetector */ - matrix::Vector3f thr_sp = _control.getThrustSetpoint(); - landdetection_thrust_limit(thr_sp); //TODO: only do that if not in pure manual - - _att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint()); - _att_sp.yaw_sp_move_rate = _control.getYawspeedSetpoint(); + // Check if vehicle is still in smooth takeoff. + if (_in_smooth_takeoff) { + // Smooth takeoff is achieved once desired altitude/velocity setpoint is reached. + if (PX4_ISFINITE(setpoint.z)) { + _in_smooth_takeoff = _pos(2) + 0.2f > setpoint.z; + } else { + _in_smooth_takeoff = _takeoff_speed < -setpoint.vz; + } + } + + // Adjust thrust setpoint based on landdetector only if the + // vehicle is NOT in pure Manual mode. + if (!_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) { + if (_vehicle_land_detected.ground_contact) { + // Set thrust in xy to zero + thr_sp(0) = 0.0f; + thr_sp(1) = 0.0f; + // Reset integral in xy is required because PID-controller does + // know about the overwrite and would therefore increase the intragral term + _control.resetIntegralXY(); + } + + if (_vehicle_land_detected.maybe_landed) { + // we set thrust to zero + // this will help to decide if we are actually landed or not + thr_sp.zero(); + // We need to reset all integral terms otherwise the PID-controller + // will end up with wrong integral sums + _control.resetIntegralXY(); + _control.resetIntegralZ(); + } } + // Fill local position, velocity and thrust setpoint. + _local_pos_sp.timestamp = hrt_absolute_time(); + _local_pos_sp.x = _control.getPosSp()(0); + _local_pos_sp.y = _control.getPosSp()(1); + _local_pos_sp.z = _control.getPosSp()(2); + _local_pos_sp.yaw = _control.getYawSetpoint(); + _local_pos_sp.yawspeed = _control.getYawspeedSetpoint(); + + _local_pos_sp.vx = _control.getVelSp()(0); + _local_pos_sp.vy = _control.getVelSp()(1); + _local_pos_sp.vz = _control.getVelSp()(2); + thr_sp.copyTo(_local_pos_sp.thrust); + + // Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint. + _att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint()); + _att_sp.yaw_sp_move_rate = _control.getYawspeedSetpoint(); + _att_sp.fw_control_yaw = false; + _att_sp.disable_mc_yaw_control = false; + _att_sp.apply_flaps = false; + _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; + + // Publish local position setpoint (for logging only) and attitude setpoint (for attitude controller). publish_local_pos_sp(); publish_attitude(); @@ -3162,6 +3218,8 @@ MulticopterPositionControl::task_main() _control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled) { + update_smooth_takeoff(); + do_control(); /* fill local position, velocity and thrust setpoint */ @@ -3245,6 +3303,24 @@ MulticopterPositionControl::set_takeoff_velocity(float &vel_sp_z) vel_sp_z = math::max(vel_sp_z, -_takeoff_vel_limit); } +void +MulticopterPositionControl:: update_smooth_takeoff() +{ + if (!_in_smooth_takeoff && _vehicle_land_detected.landed + && _control_mode.flag_armed + && (in_auto_takeoff() || manual_wants_takeoff())) { + _in_smooth_takeoff = true; + // This ramp starts negative and goes to positive later because we want to + // be as smooth as possible. If we start at 0, we alrady jump to hover throttle. + _takeoff_vel_limit = -0.5f; + } + + else if (!_control_mode.flag_armed) { + // If we're disarmed and for some reason were in a smooth takeoff, we reset that. + _in_smooth_takeoff = false; + } +} + void MulticopterPositionControl::publish_attitude() { @@ -3291,49 +3367,6 @@ MulticopterPositionControl::publish_local_pos_sp() } } -void -MulticopterPositionControl::set_idle_state() -{ - _local_pos_sp.x = _pos(0); - _local_pos_sp.y = _pos(1); - _local_pos_sp.z = _pos(2) + 1.0f; //1m into ground when idle - _local_pos_sp.vx = 0.0f; - _local_pos_sp.vy = 0.0f; - _local_pos_sp.vz = 1.0f; //1m/s into ground - _local_pos_sp.yaw = _yaw; - _local_pos_sp.yawspeed = 0.0f; - - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = 0.0f; - _att_sp.yaw_body = _yaw; - _att_sp.yaw_sp_move_rate = 0.0f; - matrix::Quatf q_sp = matrix::Eulerf(0.0f, 0.0f, _yaw); - q_sp.copyTo(_att_sp.q_d); - _att_sp.q_d_valid = true; //TODO: check if this flag is used anywhere - _att_sp.thrust = 0.0f; -} - -void -MulticopterPositionControl::updateConstraints(Controller::Constraints &constraints) -{ - /* _contstraints */ - constraints.tilt_max = NAN; // Default no maximum tilt - - /* Set maximum tilt */ - if (!_control_mode.flag_control_manual_enabled - && _pos_sp_triplet.current.valid - && _pos_sp_triplet.current.type - == position_setpoint_s::SETPOINT_TYPE_LAND) { - - /* Auto landing tilt */ - constraints.tilt_max = _tilt_max_land; - - } else { - /* Velocity/acceleration control tilt */ - constraints.tilt_max = _tilt_max_air; - } -} - void MulticopterPositionControl::landdetection_thrust_limit(matrix::Vector3f &thrust_sp) { @@ -3365,6 +3398,27 @@ MulticopterPositionControl::landdetection_thrust_limit(matrix::Vector3f &thrust_ } } +void +MulticopterPositionControl::updateTiltConstraints(Controller::Constraints &constraints) +{ + /* _contstraints */ + constraints.tilt_max = NAN; // Default no maximum tilt + + /* Set maximum tilt */ + if (!_control_mode.flag_control_manual_enabled + && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type + == position_setpoint_s::SETPOINT_TYPE_LAND) { + + /* Auto landing tilt */ + constraints.tilt_max = _tilt_max_land; + + } else { + /* Velocity/acceleration control tilt */ + constraints.tilt_max = _tilt_max_air; + } +} + int MulticopterPositionControl::start() { diff --git a/src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp b/src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp index 3094ed61dcf7..176980bd5b3c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_tests/test_controlmath.cpp @@ -10,11 +10,13 @@ class ControlMathTest : public UnitTest private: bool testThrAttMapping(); + bool testPrioritizeVector(); }; bool ControlMathTest::run_tests() { ut_run_test(testThrAttMapping); + ut_run_test(testPrioritizeVector); return (_tests_failed == 0); } @@ -60,4 +62,42 @@ bool ControlMathTest::testThrAttMapping() return true; } +bool ControlMathTest::testPrioritizeVector() +{ + float max = 5.0f; + + // v0 already at max + matrix::Vector2f v0(max, 0); + matrix::Vector2f v1(v0(1), -v0(0)); + matrix::Vector2f v_r = ControlMath::constrainXY(v0, v1, max); + ut_assert_true(fabsf(v_r(0)) - max < FLT_EPSILON && v_r(0) > 0.0f); + ut_assert_true(fabsf(v_r(1) - 0.0f) < FLT_EPSILON); + + // v1 exceeds max but v0 is zero + v0.zero(); + v_r = ControlMath::constrainXY(v0, v1, max); + ut_assert_true(fabsf(v_r(1)) - max < FLT_EPSILON && v_r(1) < 0.0f); + ut_assert_true(fabsf(v_r(0) - 0.0f) < FLT_EPSILON); + + // v0 and v1 are below max + v0 = matrix::Vector2f(0.5f, 0.5f); + v1 = matrix::Vector2f(v0(1), -v0(0)); + v_r = ControlMath::constrainXY(v0, v1, max); + float diff = matrix::Vector2f(v_r - (v0 + v1)).length(); + ut_assert_true(diff < FLT_EPSILON); + + // v0 and v1 exceed max and are perpendicular + v0 = matrix::Vector2f(4.0f, 0.0f); + v1 = matrix::Vector2f(0.0f, -4.0f); + v_r = ControlMath::constrainXY(v0, v1, max); + ut_assert_true(v_r(0) - v0(0) < FLT_EPSILON && v_r(0) > 0.0f); + float remaining = sqrtf(max * max - (v0(0) * v0(0))); + ut_assert_true(fabsf(v_r(1)) - remaining < FLT_EPSILON && v_r(1) < FLT_EPSILON); + + //TODO: add more tests with vectors not perpendicular + + return true; + +} + ut_declare_test_c(test_controlmath, ControlMathTest) diff --git a/src/platforms/px4_module_params.h b/src/platforms/px4_module_params.h index 4ac5fd2a58e4..2db898e1a1fc 100644 --- a/src/platforms/px4_module_params.h +++ b/src/platforms/px4_module_params.h @@ -76,7 +76,7 @@ class ModuleParams : public ListNode * Call this whenever the module gets a parameter change notification. It will automatically * call updateParams() for all children, which then call updateParamsImpl(). */ - void updateParams() + virtual void updateParams() { ModuleParams *child = _children.getHead();