diff --git a/boards/beaglebone/blue/src/board_config.h b/boards/beaglebone/blue/src/board_config.h index ecaac2bd8dc9..0bfb8717b87c 100644 --- a/boards/beaglebone/blue/src/board_config.h +++ b/boards/beaglebone/blue/src/board_config.h @@ -94,4 +94,3 @@ void rc_cleaning(void); #define rc_filter_butterworth_lowpass rc_butterworth_lowpass #endif - diff --git a/platforms/posix/src/px4/common/tasks.cpp b/platforms/posix/src/px4/common/tasks.cpp index d535f34e5059..0c21396d986d 100644 --- a/platforms/posix/src/px4/common/tasks.cpp +++ b/platforms/posix/src/px4/common/tasks.cpp @@ -449,4 +449,3 @@ int px4_prctl(int option, const char *arg2, px4_task_t pid) return rv; } - diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index dd5396ccfc54..ba6adb52e522 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -56,6 +56,7 @@ add_subdirectory(output_limit) add_subdirectory(perf) add_subdirectory(pid) add_subdirectory(rc) +add_subdirectory(SlewRate) add_subdirectory(systemlib) add_subdirectory(terrain_estimation) add_subdirectory(tunes) diff --git a/src/lib/SlewRate/CMakeLists.txt b/src/lib/SlewRate/CMakeLists.txt new file mode 100644 index 000000000000..ea999bd10313 --- /dev/null +++ b/src/lib/SlewRate/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ + +px4_add_library(SlewRate + dummy.cpp +) +target_include_directories(SlewRate + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_link_libraries(SlewRate PUBLIC mathlib) + +px4_add_unit_gtest(SRC SlewRateTest.cpp LINKLIBS SlewRate) diff --git a/src/lib/SlewRate/SlewRate.hpp b/src/lib/SlewRate/SlewRate.hpp new file mode 100644 index 000000000000..441c1a70a4b3 --- /dev/null +++ b/src/lib/SlewRate/SlewRate.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 SlewRate.hpp + * + * Library limit the rate of change of a value with a maximum slew rate. + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include + +using namespace matrix; + +template +class SlewRate +{ +public: + SlewRate() = default; + ~SlewRate() = default; + + /** + * Set maximum rate of change for the value + * @param slew_rate maximum rate of change + */ + void setSlewRate(const Type slew_rate) { _slew_rate = slew_rate; } + + /** + * Set value ignoring slew rate for initialization purpose + * @param value new applied value + */ + void setForcedValue(const Type value) { _value = value; } + + /** + * Update slewrate + * @param new_value desired new value + * @param deltatime time in seconds since last update + * @return actual value that complies with the slew rate + */ + Type update(const Type new_value, const float deltatime) + { + // Limit the rate of change of the value + const Type dvalue_desired = new_value - _value; + const Type dvalue_max = _slew_rate * deltatime; + const Type dvalue = constrain(dvalue_desired, -dvalue_max, dvalue_max); + _value += dvalue; + return _value; + } + +private: + Type _slew_rate; ///< maximum rate of change for the value + Type _value; ///< state to keep last value of the slew rate + + float constrain(float value, float min, float max) + { + return math::constrain(value, min, max); + } + + Vector3f constrain(const Vector3f &value, const Vector3f &min, const Vector3f &max) + { + Vector3f constrained; + + for (int i = 0; i < 3; i++) { + constrained(i) = math::constrain(value(i), min(i), max(i)); + } + + return constrained; + } +}; diff --git a/src/lib/SlewRate/SlewRateTest.cpp b/src/lib/SlewRate/SlewRateTest.cpp new file mode 100644 index 000000000000..9b621668790e --- /dev/null +++ b/src/lib/SlewRate/SlewRateTest.cpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2019 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. + * + ****************************************************************************/ + +#include +#include + +TEST(SlewRateTest, SlewUpLimited) +{ + SlewRate _slew_rate; + _slew_rate.setSlewRate(.1f); + _slew_rate.setForcedValue(-5.5f); + + for (int i = 1; i <= 10; i++) { + EXPECT_FLOAT_EQ(_slew_rate.update(20.f, .2f), -5.5f + i * .02f); + } +} + +TEST(SlewRateTest, SlewDownLimited) +{ + SlewRate _slew_rate; + _slew_rate.setSlewRate(1.1f); + _slew_rate.setForcedValue(17.3f); + + for (int i = 1; i <= 10; i++) { + EXPECT_FLOAT_EQ(_slew_rate.update(-50.f, .3f), 17.3f - i * .33f); + } +} + +TEST(SlewRateTest, ReachValueSlewed) +{ + SlewRate _slew_rate; + _slew_rate.setSlewRate(.2f); + _slew_rate.setForcedValue(8.f); + + for (int i = 1; i <= 10; i++) { + EXPECT_FLOAT_EQ(_slew_rate.update(10.f, 1.f), 8.f + i * .2f); + } + + for (int i = 1; i <= 10; i++) { + EXPECT_FLOAT_EQ(_slew_rate.update(10.f, 1.f), 10.f); + } +} + +TEST(SlewRateTest, SlewVector3f) +{ + using namespace matrix; + + SlewRate _slew_rate; + _slew_rate.setSlewRate(Vector3f(1, 2, 3)); + _slew_rate.setForcedValue(Vector3f(4, 5, 6)); + + EXPECT_EQ(_slew_rate.update(Vector3f(100, 100, 100), 1.f), Vector3f(5, 7, 9)); + EXPECT_EQ(_slew_rate.update(Vector3f(5, 7, 9), 1.f), Vector3f(5, 7, 9)); + EXPECT_EQ(_slew_rate.update(Vector3f(-100, 100, -100), 1.f), Vector3f(4, 9, 6)); + EXPECT_EQ(_slew_rate.update(Vector3f(4.1f, 8.8f, 6.3f), 1.f), Vector3f(4.1f, 8.8f, 6.3f)); +} diff --git a/src/lib/SlewRate/dummy.cpp b/src/lib/SlewRate/dummy.cpp new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/src/lib/flight_tasks/CMakeLists.txt b/src/lib/flight_tasks/CMakeLists.txt index a97cf3edb461..30ae491b0a26 100644 --- a/src/lib/flight_tasks/CMakeLists.txt +++ b/src/lib/flight_tasks/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017 - 2019 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 @@ -64,6 +64,7 @@ list(APPEND flight_tasks_all Failsafe Descend Transition + ManualAcceleration ${flight_tasks_to_add} ) diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 6e17d8ee381f..3d489ae0a16f 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -382,7 +382,7 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory() VelocitySmoothing::timeSynchronization(_trajectory, 3); _jerk_setpoint = jerk_sp_smooth; - _acceleration_setpoint = accel_sp_smooth; + _acceleration_setpoint.setNaN(); // TODO: until smooth feed-forward is fixed _velocity_setpoint = vel_sp_smooth; _position_setpoint = pos_sp_smooth; } diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 449a84be3c4a..9d64f57dfd02 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -57,7 +57,7 @@ bool FlightTaskAutoMapper::update() // vehicle exits idle. if (_type_previous == WaypointType::idle) { - _thrust_setpoint.setNaN(); + _acceleration_setpoint.setNaN(); } // during mission and reposition, raise the landing gears but only @@ -122,7 +122,7 @@ void FlightTaskAutoMapper::_prepareIdleSetpoints() // Send zero thrust setpoint _position_setpoint.setNaN(); // Don't require any position/velocity setpoints _velocity_setpoint.setNaN(); - _thrust_setpoint.zero(); + _acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); } void FlightTaskAutoMapper::_prepareLandSetpoints() diff --git a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp b/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp index 8e948cc3394f..1edbc76d4d26 100644 --- a/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp +++ b/src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp @@ -40,7 +40,7 @@ bool FlightTaskDescend::activate(vehicle_local_position_setpoint_s last_setpoint { bool ret = FlightTask::activate(last_setpoint); // stay level to minimize horizontal drift - _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); + _acceleration_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // keep heading _yaw_setpoint = _yaw; return ret; @@ -51,12 +51,12 @@ bool FlightTaskDescend::update() if (PX4_ISFINITE(_velocity(2))) { // land with landspeed _velocity_setpoint(2) = _param_mpc_land_speed.get(); - _thrust_setpoint(2) = NAN; + _acceleration_setpoint(2) = NAN; } else { - // descend with constant thrust (crash landing) + // descend with constant acceleration (crash landing) _velocity_setpoint(2) = NAN; - _thrust_setpoint(2) = -_param_mpc_thr_hover.get() * 0.7f; + _acceleration_setpoint(2) = 0.3f; } return true; diff --git a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp index 3e5626f0bb09..03600e66ff53 100644 --- a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp +++ b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp @@ -41,7 +41,7 @@ bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s last_setpoin bool ret = FlightTask::activate(last_setpoint); _position_setpoint = _position; _velocity_setpoint.zero(); - _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_param_mpc_thr_hover.get() * 0.6f); + _acceleration_setpoint = matrix::Vector3f(0.f, 0.f, 0.3f); _yaw_setpoint = _yaw; _yawspeed_setpoint = 0.0f; return ret; @@ -52,26 +52,25 @@ bool FlightTaskFailsafe::update() if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) { // stay at current position setpoint _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f; - _thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f; + _acceleration_setpoint(0) = _acceleration_setpoint(1) = 0.0f; } else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) { // don't move along xy _position_setpoint(0) = _position_setpoint(1) = NAN; - _thrust_setpoint(0) = _thrust_setpoint(1) = NAN; + _acceleration_setpoint(0) = _acceleration_setpoint(1) = NAN; } if (PX4_ISFINITE(_position(2))) { // stay at current altitude setpoint _velocity_setpoint(2) = 0.0f; - _thrust_setpoint(2) = NAN; + _acceleration_setpoint(2) = NAN; } else if (PX4_ISFINITE(_velocity(2))) { // land with landspeed _velocity_setpoint(2) = _param_mpc_land_speed.get(); _position_setpoint(2) = NAN; - _thrust_setpoint(2) = NAN; + _acceleration_setpoint(2) = NAN; } return true; - } diff --git a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp index dcf175e73474..c8a9eadcfaac 100644 --- a/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp +++ b/src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp @@ -51,8 +51,6 @@ class FlightTaskFailsafe : public FlightTask private: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, - (ParamFloat) _param_mpc_land_speed, - (ParamFloat) - _param_mpc_thr_hover /**< throttle value at which vehicle is at hover equilibrium */ + (ParamFloat) _param_mpc_land_speed ) }; diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp index ec426f6fbd04..1bf30a874795 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp @@ -96,10 +96,12 @@ const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint() _acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration); _jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk); - _thrust_setpoint.copyTo(vehicle_local_position_setpoint.thrust); vehicle_local_position_setpoint.yaw = _yaw_setpoint; vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint; + // deprecated, only kept for output logging + matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust); + return vehicle_local_position_setpoint; } @@ -109,7 +111,6 @@ void FlightTask::_resetSetpoints() _velocity_setpoint.setNaN(); _acceleration_setpoint.setNaN(); _jerk_setpoint.setNaN(); - _thrust_setpoint.setNaN(); _yaw_setpoint = _yawspeed_setpoint = NAN; } diff --git a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp index 5bd8a751ee77..cdfdf8755be3 100644 --- a/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp @@ -165,7 +165,7 @@ class FlightTask : public ModuleParams virtual void setYawHandler(WeatherVane *ext_yaw_handler) {} void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, - const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; } + const matrix::Vector3f &thrust_sp) { _velocity_setpoint_feedback = vel_sp; } protected: uORB::SubscriptionData _sub_vehicle_local_position{ORB_ID(vehicle_local_position)}; @@ -225,12 +225,9 @@ class FlightTask : public ModuleParams matrix::Vector3f _velocity_setpoint; matrix::Vector3f _acceleration_setpoint; matrix::Vector3f _jerk_setpoint; - matrix::Vector3f _thrust_setpoint; float _yaw_setpoint{}; float _yawspeed_setpoint{}; - matrix::Vector3f _velocity_setpoint_feedback; - matrix::Vector3f _thrust_setpoint_feedback; /* Counters for estimator local position resets */ struct { diff --git a/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp b/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp index 89a240887173..32908716caf4 100644 --- a/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp +++ b/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp @@ -104,6 +104,12 @@ bool FlightTaskManual::_evaluateSticks() } } +bool FlightTaskManual::_checkTakeoff() +{ + // stick is deflected above 65% throttle (_sticks(2) is in the range [-1,1]) + return _sticks(2) < -0.3f; +} + void FlightTaskManual::_applyGearSwitch(uint8_t gswitch) { if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) { diff --git a/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp b/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp index c0ad8dab8862..61fbece1d4ff 100644 --- a/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp +++ b/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.hpp @@ -54,14 +54,13 @@ class FlightTaskManual : public FlightTask bool updateInitialize() override; protected: + bool _checkTakeoff() override; bool _sticks_data_required = true; /**< let inherited task-class define if it depends on stick data */ matrix::Vector _sticks; /**< unmodified manual stick inputs */ matrix::Vector _sticks_expo; /**< modified manual sticks using expo function*/ int _gear_switch_old = manual_control_setpoint_s::SWITCH_POS_NONE; /**< old switch state*/ - float stickDeadzone() const { return _param_mpc_hold_dz.get(); } - private: bool _evaluateSticks(); /**< checks and sets stick inputs */ diff --git a/src/lib/flight_tasks/tasks/ManualAcceleration/CMakeLists.txt b/src/lib/flight_tasks/tasks/ManualAcceleration/CMakeLists.txt new file mode 100644 index 000000000000..a60bb028ae13 --- /dev/null +++ b/src/lib/flight_tasks/tasks/ManualAcceleration/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ + +px4_add_library(FlightTaskManualAcceleration + FlightTaskManualAcceleration.cpp +) +target_include_directories(FlightTaskManualAcceleration PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(FlightTaskManualAcceleration PUBLIC FlightTaskManual FlightTaskUtility) diff --git a/src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp new file mode 100644 index 000000000000..0c291943feb1 --- /dev/null +++ b/src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 FlightTaskManualAcceleration.cpp + */ + +#include "FlightTaskManualAcceleration.hpp" +#include +#include + +using namespace matrix; + +bool FlightTaskManualAcceleration::activate(vehicle_local_position_setpoint_s last_setpoint) +{ + bool ret = FlightTaskManual::activate(last_setpoint); + _velocity_setpoint.zero(); + return ret; +} + +bool FlightTaskManualAcceleration::update() +{ + // maximum commanded acceleration and velocity + Vector3f acceleration_scale; + Vector3f velocity_scale; + acceleration_scale(0) = acceleration_scale(1) = _param_mpc_acc_hor.get(); + velocity_scale(0) = velocity_scale(1) = _param_mpc_vel_manual.get(); + + if (_velocity(2) < 0.f) { + acceleration_scale(2) = _param_mpc_acc_up_max.get(); + velocity_scale(2) = _param_mpc_z_vel_max_up.get(); + + } else { + acceleration_scale(2) = _param_mpc_acc_down_max.get(); + velocity_scale(2) = _param_mpc_z_vel_max_dn.get(); + } + + acceleration_scale *= 2.f; // because of drag the average aceleration is half + + // Yaw + _position_lock.updateYawFromStick(_yawspeed_setpoint, _yaw_setpoint, + _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime); + + // Map stick input to acceleration + Vector2f stick_xy(&_sticks_expo(0)); + _position_lock.limitStickUnitLengthXY(stick_xy); + _position_lock.rotateIntoHeadingFrameXY(stick_xy, _yaw, _yaw_setpoint); + + _acceleration_setpoint = Vector3f(stick_xy(0), stick_xy(1), _sticks_expo(2)).emult(acceleration_scale); + + // Add drag to limit speed and brake again + Vector3f cv = acceleration_scale.edivide(velocity_scale); + _acceleration_setpoint -= cv.emult(_velocity); + + lockAltitude(); + lockPosition(stick_xy.length()); + + _constraints.want_takeoff = _checkTakeoff(); + return true; +} + +void FlightTaskManualAcceleration::lockAltitude() +{ + // integrate vertical velocity + const float velocity_setpoint_z_last = _velocity_setpoint(2); + _velocity_setpoint(2) += _acceleration_setpoint(2) * _deltatime; + + if (fabsf(_sticks_expo(2)) > FLT_EPSILON) { + _position_setpoint(2) = NAN; + + } else { + if (!PX4_ISFINITE(_position_setpoint(2))) { + _position_setpoint(2) = _position(2); + } + + _position_setpoint(2) += _velocity_setpoint(2) * _deltatime; + + if (fabsf(_velocity_setpoint(2)) > fabsf(velocity_setpoint_z_last)) { + _velocity_setpoint(2) = velocity_setpoint_z_last; + } + } +} + +void FlightTaskManualAcceleration::lockPosition(const float stick_input_xy) +{ + if (stick_input_xy > FLT_EPSILON) { + _position_setpoint(0) = _position_setpoint(1) = NAN; + _velocity_setpoint(0) = _velocity_setpoint(1) = NAN; + + } else { + Vector2f position_xy(_position_setpoint); + Vector2f velocity_xy(_velocity_setpoint); + + if (!PX4_ISFINITE(position_xy(0))) { + position_xy = Vector2f(_position); + velocity_xy = Vector2f(_velocity); + } + + position_xy += Vector2f(velocity_xy) * _deltatime; + + const Vector2f velocity_xy_last = velocity_xy; + velocity_xy += Vector2f(_acceleration_setpoint) * _deltatime; + + if (velocity_xy.norm_squared() > velocity_xy_last.norm_squared()) { + velocity_xy = velocity_xy_last; + } + + _position_setpoint(0) = position_xy(0); + _position_setpoint(1) = position_xy(1); + _velocity_setpoint(0) = velocity_xy(0); + _velocity_setpoint(1) = velocity_xy(1); + } +} + +void FlightTaskManualAcceleration::_ekfResetHandlerHeading(float delta_psi) +{ + // Only reset the yaw setpoint when the heading is locked + if (PX4_ISFINITE(_yaw_setpoint)) { + _yaw_setpoint += delta_psi; + } +} diff --git a/src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp new file mode 100644 index 000000000000..f88f831109d3 --- /dev/null +++ b/src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 FlightTaskManualPosition.hpp + * + * Flight task for manual position controlled mode. + * + */ + +#pragma once + +#include "FlightTaskManual.hpp" +#include "PositionLock.hpp" + +class FlightTaskManualAcceleration : public FlightTaskManual +{ +public: + FlightTaskManualAcceleration() = default; + virtual ~FlightTaskManualAcceleration() = default; + bool activate(vehicle_local_position_setpoint_s last_setpoint) override; + bool update() override; + +protected: + PositionLock _position_lock; /**< object taking care of altitude and yaw lock logic */ + + void lockAltitude(); + void lockPosition(const float stick_input_xy); + void _ekfResetHandlerHeading(float delta_psi) override; + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual, + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_z_vel_max_up, + (ParamFloat) _param_mpc_z_vel_max_dn, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_acc_down_max, + (ParamFloat) _param_mpc_man_y_max + ) +private: + +}; diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/CMakeLists.txt b/src/lib/flight_tasks/tasks/ManualAltitude/CMakeLists.txt index 14f9ce498624..886e651d6af3 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/ManualAltitude/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskManualAltitude FlightTaskManualAltitude.cpp ) -target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTaskManual) +target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTaskManual FlightTaskUtility) target_include_directories(FlightTaskManualAltitude PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index cd30f459d599..7869bbd84626 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -38,6 +38,7 @@ #include "FlightTaskManualAltitude.hpp" #include #include +#include using namespace matrix; @@ -54,7 +55,7 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s bool ret = FlightTaskManual::activate(last_setpoint); _yaw_setpoint = NAN; _yawspeed_setpoint = 0.0f; - _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity + _acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity _position_setpoint(2) = _position(2); _velocity_setpoint(2) = 0.0f; _setDefaultConstraints(); @@ -83,9 +84,9 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s void FlightTaskManualAltitude::_scaleSticks() { - // Use stick input with deadzone, exponential curve and first order lpf for yawspeed - const float yawspeed_target = _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get()); - _yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target); + // Use sticks input with deadzone and exponential curve for vertical velocity and yawspeed + _position_lock.updateYawFromStick(_yawspeed_setpoint, _yaw_setpoint, + _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime); // Use sticks input with deadzone and exponential curve for vertical velocity const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; @@ -277,48 +278,6 @@ void FlightTaskManualAltitude::_respectGroundSlowdown() } } -void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v) -{ - float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw; - Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f)); - v(0) = v_r(0); - v(1) = v_r(1); -} - -void FlightTaskManualAltitude::_updateHeadingSetpoints() -{ - if (_isYawInput()) { - _unlockYaw(); - - } else { - _lockYaw(); - } -} - -bool FlightTaskManualAltitude::_isYawInput() -{ - /* - * A threshold larger than FLT_EPSILON is required because the - * _yawspeed_setpoint comes from an IIR filter and takes too much - * time to reach zero. - */ - return fabsf(_yawspeed_setpoint) > 0.001f; -} - -void FlightTaskManualAltitude::_unlockYaw() -{ - // no fixed heading when rotating around yaw by stick - _yaw_setpoint = NAN; -} - -void FlightTaskManualAltitude::_lockYaw() -{ - // hold the current heading when no more rotation commanded - if (!PX4_ISFINITE(_yaw_setpoint)) { - _yaw_setpoint = _yaw; - } -} - void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) { // Only reset the yaw setpoint when the heading is locked @@ -329,34 +288,17 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) void FlightTaskManualAltitude::_updateSetpoints() { - _updateHeadingSetpoints(); // get yaw setpoint - - // Thrust in xy are extracted directly from stick inputs. A magnitude of - // 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no - // thrust along xy is demanded. The maximum thrust along xy depends on the thrust - // setpoint along z-direction, which is computed in PositionControl.cpp. - - Vector2f sp(&_sticks(0)); - _rotateIntoHeadingFrame(sp); - - if (sp.length() > 1.0f) { - sp.normalize(); - } - - _thrust_setpoint(0) = sp(0); - _thrust_setpoint(1) = sp(1); - _thrust_setpoint(2) = NAN; + // Extraact acceleration from sticks with the maximum tilt as scale + Vector2f stick_xy(&_sticks(0)); + _position_lock.limitStickUnitLengthXY(stick_xy); + _position_lock.rotateIntoHeadingFrameXY(stick_xy, _yaw, _yaw_setpoint); + _acceleration_setpoint = Vector3f(stick_xy(0), stick_xy(1), NAN); + _acceleration_setpoint *= tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G; _updateAltitudeLock(); _respectGroundSlowdown(); } -bool FlightTaskManualAltitude::_checkTakeoff() -{ - // stick is deflected above 65% throttle (_sticks(2) is in the range [-1,1]) - return _sticks(2) < -0.3f; -} - bool FlightTaskManualAltitude::update() { _scaleSticks(); diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 512c1811175e..470f29828c98 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -40,6 +40,7 @@ #pragma once #include "FlightTaskManual.hpp" +#include "PositionLock.hpp" class FlightTaskManualAltitude : public FlightTaskManual { @@ -51,11 +52,9 @@ class FlightTaskManualAltitude : public FlightTaskManual bool update() override; protected: - void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ virtual void _updateSetpoints(); /**< updates all setpoints */ virtual void _scaleSticks(); /**< scales sticks to velocity in z */ - bool _checkTakeoff() override; /** * rotates vector into local frame @@ -69,6 +68,8 @@ class FlightTaskManualAltitude : public FlightTaskManual */ void _updateAltitudeLock(); + PositionLock _position_lock; /**< object taking care of altitude and yaw lock logic */ + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual, (ParamFloat) _param_mpc_hold_max_z, (ParamInt) _param_mpc_alt_mode, diff --git a/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index 7bc4c90a9246..a522a7122a1c 100644 --- a/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -115,7 +115,7 @@ void FlightTaskManualAltitudeSmoothVel::_updateTrajConstraints() void FlightTaskManualAltitudeSmoothVel::_setOutputState() { _jerk_setpoint(2) = _smoothing.getCurrentJerk(); - _acceleration_setpoint(2) = _smoothing.getCurrentAcceleration(); + _acceleration_setpoint(2) = NAN; // TODO: until smooth feed-forward is fixed _velocity_setpoint(2) = _smoothing.getCurrentVelocity(); _position_setpoint(2) = _smoothing.getCurrentPosition(); } diff --git a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 57724e5f33c0..ad7395c9c61e 100644 --- a/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -84,13 +84,9 @@ void FlightTaskManualPosition::_scaleSticks() FlightTaskManualAltitude::_scaleSticks(); /* Constrain length of stick inputs to 1 for xy*/ - Vector2f stick_xy = _sticks_expo.slice<2, 1>(0, 0); - - const float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f); - - if (mag > FLT_EPSILON) { - stick_xy = stick_xy.normalized() * mag; - } + Vector2f vel_sp_xy = _sticks_expo.slice<2, 1>(0, 0); + _position_lock.limitStickUnitLengthXY(vel_sp_xy); + _position_lock.rotateIntoHeadingFrameXY(vel_sp_xy, _yaw, _yaw_setpoint); const float max_speed_from_estimator = _sub_vehicle_local_position.get().vxy_max; @@ -107,10 +103,7 @@ void FlightTaskManualPosition::_scaleSticks() _velocity_scale = fminf(_computeVelXYGroundDist(), _velocity_scale); // scale velocity to its maximum limits - Vector2f vel_sp_xy = stick_xy * _velocity_scale; - - /* Rotate setpoint into local frame. */ - _rotateIntoHeadingFrame(vel_sp_xy); + vel_sp_xy *= _velocity_scale; // collision prevention if (_collision_prevention.is_active()) { @@ -165,6 +158,7 @@ void FlightTaskManualPosition::_updateXYlock() void FlightTaskManualPosition::_updateSetpoints() { FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction + _acceleration_setpoint.setNaN(); // check if an external yaw handler is active and if yes, let it update the yaw setpoints if (_weathervane_yaw_handler != nullptr && _weathervane_yaw_handler->is_active()) { @@ -172,6 +166,5 @@ void FlightTaskManualPosition::_updateSetpoints() _yawspeed_setpoint += _weathervane_yaw_handler->get_weathervane_yawrate(); } - _thrust_setpoint.setAll(NAN); // don't require any thrust setpoints _updateXYlock(); // check for position lock } diff --git a/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index b526a2769491..0b1589de3ce6 100644 --- a/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -172,7 +172,7 @@ void FlightTaskManualPositionSmoothVel::_setOutputState() void FlightTaskManualPositionSmoothVel::_setOutputStateXY() { _jerk_setpoint.xy() = _smoothing_xy.getCurrentJerk(); - _acceleration_setpoint.xy() = _smoothing_xy.getCurrentAcceleration(); + _acceleration_setpoint.xy() = Vector2f(NAN, NAN); // TODO: until smooth feed-forward is fixed _velocity_setpoint.xy() = _smoothing_xy.getCurrentVelocity(); _position_setpoint.xy() = _smoothing_xy.getCurrentPosition(); } @@ -180,7 +180,7 @@ void FlightTaskManualPositionSmoothVel::_setOutputStateXY() void FlightTaskManualPositionSmoothVel::_setOutputStateZ() { _jerk_setpoint(2) = _smoothing_z.getCurrentJerk(); - _acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration(); + _acceleration_setpoint(2) = NAN; // TODO: until smooth feed-forward is fixed _velocity_setpoint(2) = _smoothing_z.getCurrentVelocity(); _position_setpoint(2) = _smoothing_z.getCurrentPosition(); } diff --git a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp b/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp index bd1fbc0c7eff..89b49963a9e1 100644 --- a/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp +++ b/src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp @@ -150,7 +150,7 @@ bool FlightTaskOffboard::update() // IDLE if (_sub_triplet_setpoint.get().current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { - _thrust_setpoint.zero(); + _acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); return true; } @@ -228,9 +228,9 @@ bool FlightTaskOffboard::update() // Acceleration // Note: this is not supported yet and will be mapped to normalized thrust directly. if (_sub_triplet_setpoint.get().current.acceleration_valid) { - _thrust_setpoint(0) = _sub_triplet_setpoint.get().current.a_x; - _thrust_setpoint(1) = _sub_triplet_setpoint.get().current.a_y; - _thrust_setpoint(2) = _sub_triplet_setpoint.get().current.a_z; + _acceleration_setpoint(0) = _sub_triplet_setpoint.get().current.a_x; + _acceleration_setpoint(1) = _sub_triplet_setpoint.get().current.a_y; + _acceleration_setpoint(2) = _sub_triplet_setpoint.get().current.a_z; } // use default conditions of upwards position or velocity to take off diff --git a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp index 1f2a0c9cb39a..e39ea6f49f85 100644 --- a/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp +++ b/src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp @@ -79,7 +79,7 @@ bool FlightTaskTransition::update() { // level wings during the transition, altitude should be controlled _position_setpoint(2) = _transition_altitude; - _thrust_setpoint.xy() = matrix::Vector2f(0.f, 0.f); + _acceleration_setpoint.xy() = matrix::Vector2f(0.f, 0.f); updateAccelerationEstimate(); diff --git a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt index 287f24f22be6..d2d9b1f8c300 100644 --- a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt @@ -39,9 +39,10 @@ px4_add_library(FlightTaskUtility VelocitySmoothing.cpp ManualVelocitySmoothingXY.cpp ManualVelocitySmoothingZ.cpp + PositionLock.hpp ) -target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis) +target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis SlewRate) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS FlightTaskUtility) diff --git a/src/lib/flight_tasks/tasks/Utility/PositionLock.hpp b/src/lib/flight_tasks/tasks/Utility/PositionLock.hpp new file mode 100644 index 000000000000..88e717dc1449 --- /dev/null +++ b/src/lib/flight_tasks/tasks/Utility/PositionLock.hpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 PositionLock.hpp + * + * Library for position and yaw lock logic when flying by stick. + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include "SlewRate.hpp" + +class PositionLock +{ +public: + PositionLock() = default; + ~PositionLock() = default; + + /** + * Lock yaw when not currently turning + * When applying a yawspeed the vehicle is turning, when the speed is + * set to zero the vehicle needs to slow down and then lock at the yaw + * it stops at to not drift over time. + * @param yaw_stick_input current yaw rotational rate state + * @return yaw setpoint to execute to have a yaw lock at the correct moment in time + */ + void updateYawFromStick(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, const float yaw, + const float deltatime) + { + _yawspeed_slew_rate.setSlewRate(2.f * M_PI_F); + yawspeed_setpoint = _yawspeed_slew_rate.update(desired_yawspeed, deltatime); + yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint); + } + + /** + * Lock yaw when not currently turning + * When applying a yawspeed the vehicle is turning, when the speed is + * set to zero the vehicle needs to slow down and then lock at the yaw + * it stops at to not drift over time. + * @param yawspeed current yaw rotational rate state + * @param yaw current yaw rotational rate state + * @param yawspeed_setpoint rotation rate at which to turn around yaw axis + * @param yaw current yaw setpoint which then will be overwritten by the return value + * @return yaw setpoint to execute to have a yaw lock at the correct moment in time + */ + static float updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint) + { + // Yaw-lock depends on desired yawspeed input. If not locked, yaw_sp is set to NAN. + if (fabsf(yawspeed_setpoint) > FLT_EPSILON) { + // no fixed heading when rotating around yaw by stick + return NAN; + + } else { + // break down and hold the current heading when no more rotation commanded + if (!PX4_ISFINITE(yaw_setpoint)) { + return yaw; + + } else { + return yaw_setpoint; + } + } + } + + /** + * Limit the the horizontal input from a square shaped joystick gimbal to a unit circle + * @param v Vector containing x, y, z axis of the joystick gimbal. x, y get adjusted + */ + void limitStickUnitLengthXY(matrix::Vector2f &v) + { + const float vl = v.length(); + + if (vl > 1.0f) { + v(1) /= vl; + v(2) /= vl; + } + } + + /** + * Rotate horizontal component of joystick input into the vehicle body orientation + * @param v Vector containing x, y, z axis of the joystick input. + * @param yaw Current vehicle yaw heading + * @param yaw_setpoint Current yaw setpoint if it's locked else NAN + */ + void rotateIntoHeadingFrameXY(matrix::Vector2f &v, const float yaw, const float yaw_setpoint) + { + using namespace matrix; + // Rotate horizontal acceleration input to body heading + Vector3f v3(v(0), v(1), 0.f); + const float yaw_rotate = PX4_ISFINITE(yaw_setpoint) ? yaw_setpoint : yaw; + v = Vector2f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * v3); + } + +private: + SlewRate _yawspeed_slew_rate; + +}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 007fdb439a4e..af6b532cbeb7 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -394,4 +394,3 @@ void LoggedTopics::initialize_configured_topics(SDLogProfileMask profile) add_vision_and_avoidance_topics(); } } - diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index a0638d3415bb..0d3754a5ca66 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -50,6 +50,36 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitu att_sp.thrust_body[2] = -thr_sp.length(); } +void accelerationToAttitude(const Vector3f &acc_sp, const float yaw_sp, const float hover_thrust, const float tilt_max, + vehicle_attitude_setpoint_s &att_sp) +{ + // Assume standard acceleration due to gravity in vertical direction for attitude generation + Vector3f body_z_unit = Vector3f(-acc_sp(0), -acc_sp(1), CONSTANTS_ONE_G).unit(); + limitTilt(body_z_unit, Vector3f(0, 0, 1), tilt_max); + bodyzToAttitude(body_z_unit, yaw_sp, att_sp); + // Scale thrust assuming hover thrust produces standard gravity + att_sp.thrust_body[2] = acc_sp(2) * (hover_thrust / CONSTANTS_ONE_G) - hover_thrust; + // Project thrust to planned body attitude + att_sp.thrust_body[2] /= (Vector3f(0, 0, 1).dot(body_z_unit)); +} + +void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_angle) +{ + // determine tilt + const float dot_product_unit = body_unit.dot(world_unit); + float angle = acosf(dot_product_unit); + // limit tilt + angle = math::min(angle, max_angle); + Vector3f rejection = body_unit - (dot_product_unit * world_unit); + + // corner case exactly parallel vectors + if (rejection.norm_squared() < FLT_EPSILON) { + rejection(0) = 1.f; + } + + body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit(); +} + void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) { // zero vector, no direction, set safe level value @@ -159,7 +189,6 @@ Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) // notes: // - s (=scaling factor) needs to be positive // - (max - ||v||) always larger than zero, otherwise it never entered this if-statement - Vector2f u1 = v1.normalized(); float m = u1.dot(v0); float c = v0.dot(v0) - max * max; @@ -215,4 +244,32 @@ bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r, return false; } } + +void addIfNotNan(float &setpoint, const float addition) +{ + if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(addition)) { + // No NAN, add to the setpoint + setpoint += addition; + + } else if (!PX4_ISFINITE(setpoint)) { + // Setpoint NAN, take addition + setpoint = addition; + } + + // Addition is NAN or both are NAN, nothing to do } + +void addIfNotNanVector(Vector3f &setpoint, const Vector3f &addition) +{ + for (int i = 0; i < 3; i++) { + addIfNotNan(setpoint(i), addition(i)); + } +} + +void setZeroIfNanVector(Vector3f &vector) +{ + // Adding zero vector overwrites elements that are NaN with zero + addIfNotNanVector(vector, Vector3f()); +} + +} // ControlMath diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp index eed3e483c3ea..33668d484be3 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp @@ -42,6 +42,7 @@ #include #include +#include namespace ControlMath { @@ -52,6 +53,25 @@ namespace ControlMath * @param att_sp attitude setpoint to fill */ void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); + +/** + * Converts acceleration vector and yaw set-point to a desired attitude. + * @param acc_sp a 3D vector + * @param yaw_sp the desired yaw setpoint + * @param tilt_max maximum allowed tilt angle in radians + * @param att_sp attitude setpoint to fill + */ +void accelerationToAttitude(const matrix::Vector3f &acc_sp, const float yaw_sp, const float hover_thrust, + const float tilt_max, vehicle_attitude_setpoint_s &att_sp); + +/** + * Limits the tilt angle between two unit vectors + * @param body_unit unit vector that will get adjusted if angle is to big + * @param world_unit vector to measure the angle against + * @param max_angle maximum angle between vectors in radians + */ +void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit, const float max_angle); + /** * Converts a body z vector and yaw set-point to a desired attitude. * @param body_z a world frame 3D vector in direction of the desired body z axis @@ -86,4 +106,24 @@ matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f */ bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r, const matrix::Vector3f &line_a, const matrix::Vector3f &line_b, matrix::Vector3f &res); + +/** + * Adds e.g. feed-forward to the setpoint making sure existing or added NANs have no influence on control. + * This function is udeful to support all the different setpoint combinations of position, velocity, acceleration with NAN representing an uncommited value. + * @param setpoint existing possibly NAN setpoint to add to + * @param addition value/NAN to add to the setpoint + */ +void addIfNotNan(float &setpoint, const float addition); + +/** + * _addIfNotNan for Vector3f treating each element individually + * @see _addIfNotNan + */ +void addIfNotNanVector(matrix::Vector3f &setpoint, const matrix::Vector3f &addition); + +/** + * Overwrites elements of a Vector3f which are NaN with zero + * @param vector possibly containing NAN elements + */ +void setZeroIfNanVector(matrix::Vector3f &vector); } diff --git a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp index 2623ba69f149..760ee2b9628a 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp @@ -38,6 +38,65 @@ using namespace matrix; using namespace ControlMath; +TEST(ControlMathTest, LimitTiltUnchanged) +{ + Vector3f body = Vector3f(0, 0, 1).normalized(); + Vector3f body_before = body; + limitTilt(body, Vector3f(0, 0, 1), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); + + body = Vector3f(0, .1f, 1).normalized(); + body_before = body; + limitTilt(body, Vector3f(0, 0, 1), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); +} + +TEST(ControlMathTest, LimitTiltOpposite) +{ + Vector3f body = Vector3f(0, 0, -1).normalized(); + limitTilt(body, Vector3f(0, 0, 1), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0, 0, 1))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTiltAlmostOpposite) +{ + // This case doesn't trigger corner case handling but is very close to it + Vector3f body = Vector3f(0.001f, 0, -1.f).normalized(); + limitTilt(body, Vector3f(0, 0, 1), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0, 0, 1))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTilt45degree) +{ + Vector3f body = Vector3f(1, 0, 0); + limitTilt(body, Vector3f(0, 0, 1), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(M_SQRT1_2_F, 0, M_SQRT1_2_F)); + + body = Vector3f(0, 1, 0); + limitTilt(body, Vector3f(0, 0, 1), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(0, M_SQRT1_2_F, M_SQRT1_2_F)); +} + +TEST(ControlMathTest, LimitTilt10degree) +{ + Vector3f body = Vector3f(1, 1, .1f).normalized(); + limitTilt(body, Vector3f(0, 0, 1), M_DEG_TO_RAD_F * 10.f); + float angle = acosf(body.dot(Vector3f(0, 0, 1))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(body(0), body(1)); + + body = Vector3f(1, 2, .2f); + limitTilt(body, Vector3f(0, 0, 1), M_DEG_TO_RAD_F * 10.f); + angle = acosf(body.dot(Vector3f(0, 0, 1))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(2.f * body(0), body(1)); +} TEST(ControlMathTest, ThrottleAttitudeMapping) { @@ -177,3 +236,21 @@ TEST(ControlMathTest, CrossSphereLine) EXPECT_FALSE(retval); EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); } + +TEST(ControlMathTest, addIfNotNan) +{ + float v = 1.f; + // regular addition + ControlMath::addIfNotNan(v, 2.f); + EXPECT_EQ(v, 3.f); + // addition is NAN and has no influence + ControlMath::addIfNotNan(v, NAN); + EXPECT_EQ(v, 3.f); + v = NAN; + // both summands are NAN + ControlMath::addIfNotNan(v, NAN); + EXPECT_TRUE(isnan(v)); + // regular value gets added to NAN and overwrites it + ControlMath::addIfNotNan(v, 3.f); + EXPECT_EQ(v, 3.f); +} diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index ff67df4b6126..306611236c83 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -59,8 +59,10 @@ void PositionControl::setVelocityLimits(const float vel_horizontal, const float void PositionControl::setThrustLimits(const float min, const float max) { - _lim_thr_min = min; - _lim_thr_max = max; + // convert the parameter input interface [0,1] to NED frame [-1,0] + // make sure the thrust vector has a tiny minimal length to infer the direction + _lim_thr_min = -max; + _lim_thr_max = -math::max(min, -10e-4f); } void PositionControl::setState(const PositionControlStates &states) @@ -71,7 +73,7 @@ void PositionControl::setState(const PositionControlStates &states) _vel_dot = states.acceleration; } -bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint) +void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint) { _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); @@ -79,14 +81,6 @@ bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s & _thr_sp = Vector3f(setpoint.thrust); _yaw_sp = setpoint.yaw; _yawspeed_sp = setpoint.yawspeed; - bool mapping_succeeded = _interfaceMapping(); - - // If full manual is required (thrust already generated), don't run position/velocity - // controller and just return thrust. - _skip_controller = PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) - && PX4_ISFINITE(_thr_sp(2)); - - return mapping_succeeded; } void PositionControl::setConstraints(const vehicle_constraints_s &constraints) @@ -110,242 +104,98 @@ void PositionControl::setConstraints(const vehicle_constraints_s &constraints) // ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion } -void PositionControl::update(const float dt) +bool PositionControl::update(const float dt) { - if (_skip_controller) { - // Already received a valid thrust set-point. - // Limit the thrust vector. - float thr_mag = _thr_sp.length(); - - if (thr_mag > _lim_thr_max) { - _thr_sp = _thr_sp.normalized() * _lim_thr_max; - - } else if (thr_mag < _lim_thr_min && thr_mag > FLT_EPSILON) { - _thr_sp = _thr_sp.normalized() * _lim_thr_min; - } - - // Just set the set-points equal to the current vehicle state. - _pos_sp = _pos; - _vel_sp = _vel; - _acc_sp = _vel_dot; - return; - } - _positionControl(); _velocityControl(dt); -} - -bool PositionControl::_interfaceMapping() -{ - // if nothing is valid, then apply failsafe landing - bool failsafe = false; - - // Respects FlightTask interface, where NAN-set-points are of no interest - // and do not require control. A valid position and velocity setpoint will - // be mapped to a desired position setpoint with a feed-forward term. - // States and setpoints which are integrals of the reference setpoint are set to 0. - // For instance: reference is velocity-setpoint -> position and position-setpoint = 0 - // reference is thrust-setpoint -> position, velocity, position-/velocity-setpoint = 0 - for (int i = 0; i <= 2; i++) { - - if (PX4_ISFINITE(_pos_sp(i))) { - // Position control is required - - if (!PX4_ISFINITE(_vel_sp(i))) { - // Velocity is not used as feedforward term. - _vel_sp(i) = 0.0f; - } - - // thrust setpoint is not supported in position control - _thr_sp(i) = NAN; - - // to run position control, we require valid position and velocity - if (!PX4_ISFINITE(_pos(i)) || !PX4_ISFINITE(_vel(i))) { - failsafe = true; - } - - } else if (PX4_ISFINITE(_vel_sp(i))) { - - // Velocity controller is active without position control. - // Set integral states and setpoints to 0 - _pos_sp(i) = _pos(i) = 0.0f; - - // thrust setpoint is not supported in velocity control - _thr_sp(i) = NAN; - - // to run velocity control, we require valid velocity - if (!PX4_ISFINITE(_vel(i))) { - failsafe = true; - } - - } else if (PX4_ISFINITE(_thr_sp(i))) { - - // Thrust setpoint was generated from sticks directly. - // Set all integral states and setpoints to 0 - _pos_sp(i) = _pos(i) = 0.0f; - _vel_sp(i) = _vel(i) = 0.0f; - - // Reset the Integral term. - _vel_int(i) = 0.0f; - // Don't require velocity derivative. - _vel_dot(i) = 0.0f; - - } else { - // nothing is valid. do failsafe - failsafe = true; - } - } - - // ensure that vel_dot is finite, otherwise set to 0 - if (!PX4_ISFINITE(_vel_dot(0)) || !PX4_ISFINITE(_vel_dot(1))) { - _vel_dot(0) = _vel_dot(1) = 0.0f; - } - - if (!PX4_ISFINITE(_vel_dot(2))) { - _vel_dot(2) = 0.0f; - } - - 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. - if (PX4_ISFINITE(_yaw)) { - _yaw_sp = _yaw; + _yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f; + _yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; - } else { - failsafe = true; - } - } - - // check failsafe - if (failsafe) { - // point the thrust upwards - _thr_sp(0) = _thr_sp(1) = 0.0f; - // throttle down such that vehicle goes down with - // 70% of throttle range between min and hover - _thr_sp(2) = -(_lim_thr_min + (_hover_thrust - _lim_thr_min) * 0.7f); - // position and velocity control-loop is currently unused (flag only for logging purpose) - } - - return !(failsafe); + // if the acceleration setpoint is valid there was a setpoint state pair + // for each dimension that can get controlled + const bool controller_output_is_valid = PX4_ISFINITE(_acc_sp(0)) + && PX4_ISFINITE(_acc_sp(1)) + && PX4_ISFINITE(_acc_sp(2)); + return controller_output_is_valid; } void PositionControl::_positionControl() { - // P-position controller - const Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); - _vel_sp = vel_sp_position + _vel_sp; + Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); + ControlMath::addIfNotNanVector(_vel_sp, vel_sp_position); + ControlMath::setZeroIfNanVector(vel_sp_position); // Constrain horizontal velocity by prioritizing the velocity component along the // the desired position setpoint over the feed-forward term. - const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position), - Vector2f(_vel_sp - vel_sp_position), _lim_vel_horizontal); - _vel_sp(0) = vel_sp_xy(0); - _vel_sp(1) = vel_sp_xy(1); + _vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal); // Constrain velocity in z-direction. _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down); } void PositionControl::_velocityControl(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. - - const Vector3f vel_err = _vel_sp - _vel; - - // Consider thrust in D-direction. - float thrust_desired_D = _gain_vel_p(2) * vel_err(2) + _gain_vel_d(2) * _vel_dot(2) + _vel_int( - 2) - _hover_thrust; - - // The Thrust limits are negated and swapped due to NED-frame. - float uMax = -_lim_thr_min; - float uMin = -_lim_thr_max; - - // make sure there's always enough thrust vector length to infer the attitude - uMax = math::min(uMax, -10e-4f); - - // 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) { - _vel_int(2) += vel_err(2) * _gain_vel_i(2) * dt; - - // limit thrust integral - _vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * math::sign(_vel_int(2)); + const float hover_scale = CONSTANTS_ONE_G / _hover_thrust; + + // PID velocity control + Vector3f vel_error = _vel_sp - _vel; + Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int + _vel_dot.emult(_gain_vel_d); + + // For backwards compatibility of the gains + acc_sp_velocity *= hover_scale; + // No control input from setpoints or corresponding states which are NAN + ControlMath::addIfNotNanVector(_acc_sp, acc_sp_velocity); + + _accelerationControl(); + + // Apply Anti-Windup in vertical direction + if ((_thr_sp(2) >= _lim_thr_max && vel_error(2) >= 0.0f) || + (_thr_sp(2) <= _lim_thr_min && vel_error(2) <= 0.0f)) { + vel_error(2) = 0.f; } - // Saturate thrust setpoint in D-direction. - _thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax); - - if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) { - // 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(_constraints.tilt); - _thr_sp(0) *= thr_xy_max; - _thr_sp(1) *= thr_xy_max; - - } else { - // PID-velocity controller for NE-direction. - Vector2f thrust_desired_NE; - thrust_desired_NE(0) = _gain_vel_p(0) * vel_err(0) + _gain_vel_d(0) * _vel_dot(0) + _vel_int(0); - thrust_desired_NE(1) = _gain_vel_p(1) * vel_err(1) + _gain_vel_d(1) * _vel_dot(1) + _vel_int(1); - - // Get maximum allowed thrust in NE based on tilt and excess thrust. - float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt); - float thrust_max_NE = sqrtf(_lim_thr_max * _lim_thr_max - _thr_sp(2) * _thr_sp(2)); - thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE); - - // Saturate thrust in NE-direction. - _thr_sp(0) = thrust_desired_NE(0); - _thr_sp(1) = thrust_desired_NE(1); - - if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) { - float mag = thrust_desired_NE.length(); - _thr_sp(0) = thrust_desired_NE(0) / mag * thrust_max_NE; - _thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE; - } - - // Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output - // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 - float arw_gain = 2.f / _gain_vel_p(0); - - Vector2f vel_err_lim; - vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain; - vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain; - - // Update integral - _vel_int(0) += _gain_vel_i(0) * vel_err_lim(0) * dt; - _vel_int(1) += _gain_vel_i(1) * vel_err_lim(1) * dt; + // Saturate thrust setpoint in vertical direction + _thr_sp(2) = math::max(_thr_sp(2), _lim_thr_min); + + // Get allowed horizontal thrust limit based on excess thrust + const float thrust_xy_min_squared = _lim_thr_min * _lim_thr_min; + const float thrust_z_squared = _thr_sp(2) * _thr_sp(2); + float thrust_min_xy = sqrtf(thrust_xy_min_squared - thrust_z_squared); + + // Saturate thrust in horizontal direction. + Vector2f thrust_sp_xy(_thr_sp); + + if (thrust_sp_xy.norm_squared() > thrust_min_xy * thrust_min_xy) { + thrust_sp_xy = thrust_sp_xy.normalized() * thrust_min_xy; + _thr_sp(0) = thrust_sp_xy(0); + _thr_sp(1) = thrust_sp_xy(1); } + + // Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output + // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 + const float arw_gain = 2.f / _gain_vel_p(0); + const Vector2f vel_error_lim = Vector2f(vel_error) - (arw_gain * (thrust_sp_xy - Vector2f(_thr_sp))); + vel_error(0) = vel_error_lim(0); + vel_error(1) = vel_error_lim(1); + + // Update integral part of velocity control + _vel_int += vel_error * _gain_vel_i * dt; + // Make sure integral doesn't stay NAN + ControlMath::setZeroIfNanVector(_vel_int); } +void PositionControl::_accelerationControl() +{ + // Assume standard acceleration due to gravity in vertical direction for attitude generation + Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized(); + ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt); + // Scale thrust assuming hover thrust produces standard gravity + float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; + // Project thrust to planned body attitude + collective_thrust /= (Vector3f(0, 0, 1).dot(body_z)); + collective_thrust = math::min(collective_thrust, _lim_thr_max); + _thr_sp = body_z * collective_thrust; +} void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const { @@ -363,6 +213,7 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const { - ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint); + ControlMath::bodyzToAttitude(-_thr_sp, _yaw_sp, attitude_setpoint); + attitude_setpoint.thrust_body[2] = -_thr_sp.length(); attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 07cf0b03f67e..697c29346e22 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -129,9 +129,8 @@ class PositionControl * Pass the desired setpoints * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint. * @param setpoint a vehicle_local_position_setpoint_s structure - * @return true if a valid setpoint was set */ - bool setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint); + void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint); /** * Pass constraints that are stricter than the global limits @@ -149,7 +148,7 @@ class PositionControl * @param dt time in seconds since last iteration * @return true if output setpoint is executable, false if not */ - void update(const float dt); + bool update(const float dt); /** * Set the integral term in xy to 0. @@ -174,14 +173,9 @@ class PositionControl void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const; private: - /** - * Maps setpoints to internal-setpoints. - * @return true if mapping succeeded. - */ - bool _interfaceMapping(); - void _positionControl(); ///< Position proportional control void _velocityControl(const float dt); ///< Velocity PID control + void _accelerationControl(); ///< Acceleration setpoint processing // Gains matrix::Vector3f _gain_pos_p; ///< Position control proportional gain @@ -215,6 +209,4 @@ class PositionControl matrix::Vector3f _thr_sp; /**< desired thrust */ float _yaw_sp{}; /**< desired heading */ float _yawspeed_sp{}; /** desired yaw-speed */ - - bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */ }; diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 1ba6372684fa..e75154bcfde7 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -133,7 +133,7 @@ class PositionControlBasicDirectionTest : public PositionControlBasicTest } }; -TEST_F(PositionControlBasicDirectionTest, PositionDirection) +TEST_F(PositionControlBasicDirectionTest, PositionControlDirectionP) { _input_setpoint.x = .1f; _input_setpoint.y = .1f; @@ -142,7 +142,7 @@ TEST_F(PositionControlBasicDirectionTest, PositionDirection) checkDirection(); } -TEST_F(PositionControlBasicDirectionTest, VelocityDirection) +TEST_F(PositionControlBasicDirectionTest, VelocityControlDirection) { _input_setpoint.vx = .1f; _input_setpoint.vy = .1f; @@ -151,7 +151,14 @@ TEST_F(PositionControlBasicDirectionTest, VelocityDirection) checkDirection(); } -TEST_F(PositionControlBasicTest, TiltLimit) +TEST_F(PositionControlBasicDirectionTest, AccelerationControlDirection) +{ + Vector3f(1, 1, -1).copyTo(_input_setpoint.acceleration); + runController(); + checkDirection(); +} + +TEST_F(PositionControlBasicTest, PositionControlMaxTilt) { _input_setpoint.x = 10.f; _input_setpoint.y = 10.f; @@ -171,7 +178,7 @@ TEST_F(PositionControlBasicTest, TiltLimit) EXPECT_LE(angle, .50001f); } -TEST_F(PositionControlBasicTest, VelocityLimit) +TEST_F(PositionControlBasicTest, PositionControlMaxVelocity) { _input_setpoint.x = 10.f; _input_setpoint.y = 10.f; @@ -183,65 +190,51 @@ TEST_F(PositionControlBasicTest, VelocityLimit) EXPECT_LE(abs(_output_setpoint.vz), 1.f); } -TEST_F(PositionControlBasicTest, ThrustLimit) +TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) { _input_setpoint.x = 10.f; _input_setpoint.y = 10.f; _input_setpoint.z = -10.f; runController(); - EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f); - EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f); - EXPECT_LT(_attitude.thrust_body[2], -.1f); - EXPECT_GE(_attitude.thrust_body[2], -0.9f); -} + Vector3f thrust(_output_setpoint.thrust); + EXPECT_FLOAT_EQ(thrust(0), 0.f); + EXPECT_FLOAT_EQ(thrust(1), 0.f); + EXPECT_FLOAT_EQ(thrust(2), -0.9f); -TEST_F(PositionControlBasicTest, FailsafeInput) -{ - _input_setpoint.vz = .7f; - _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f; + EXPECT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.9f); - runController(); - EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f); - EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f); - EXPECT_LT(_output_setpoint.thrust[2], -.1f); - EXPECT_GT(_output_setpoint.thrust[2], -.5f); - EXPECT_GT(_attitude.thrust_body[2], -.5f); - EXPECT_LE(_attitude.thrust_body[2], -.1f); + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); } -TEST_F(PositionControlBasicTest, InputCombinationsPosition) +TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) { - _input_setpoint.x = .1f; - _input_setpoint.y = .2f; - _input_setpoint.z = .3f; + _input_setpoint.x = 10.f; + _input_setpoint.y = 0.f; + _input_setpoint.z = 10.f; runController(); - EXPECT_FLOAT_EQ(_output_setpoint.x, .1f); - EXPECT_FLOAT_EQ(_output_setpoint.y, .2f); - EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); - EXPECT_FALSE(isnan(_output_setpoint.vx)); - EXPECT_FALSE(isnan(_output_setpoint.vy)); - EXPECT_FALSE(isnan(_output_setpoint.vz)); - EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); - EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); - EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); + Vector3f thrust(_output_setpoint.thrust); + EXPECT_FLOAT_EQ(thrust.length(), 0.1f); + + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f); + + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f); } -TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity) +TEST_F(PositionControlBasicTest, PositionControlFailsafeInput) { - _input_setpoint.vx = .1f; - _input_setpoint.vy = .2f; - _input_setpoint.z = .3f; // altitude + _input_setpoint.vz = .7f; + _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f; runController(); - // EXPECT_TRUE(isnan(_output_setpoint.x)); - // EXPECT_TRUE(isnan(_output_setpoint.y)); - EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); - EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f); - EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f); - EXPECT_FALSE(isnan(_output_setpoint.vz)); - EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); - EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); - EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); + EXPECT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_GT(_output_setpoint.acceleration[2], 0.f); + EXPECT_GT(_attitude.thrust_body[2], -.5f); + EXPECT_LE(_attitude.thrust_body[2], -.1f); } 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 10e5991e1f15..54d865311583 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -233,13 +233,6 @@ class MulticopterPositionControl : public ModuleBase */ void warn_rate_limited(const char *str); - /** - * Adjust the setpoint during landing. - * Thrust is adjusted to support the land-detector during detection. - * @param setpoint gets adjusted based on land-detector state - */ - void limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint); - /** * Start flightasks based on navigation state. * This methods activates a task based on the navigation state. @@ -563,19 +556,6 @@ MulticopterPositionControl::Run() constraints = _flight_tasks.getConstraints(); _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now); - - // Check if position, velocity or thrust pairs are valid -> trigger failsaife if no pair is valid - if (!(PX4_ISFINITE(setpoint.x) && PX4_ISFINITE(setpoint.y)) && - !(PX4_ISFINITE(setpoint.vx) && PX4_ISFINITE(setpoint.vy)) && - !(PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]))) { - failsafe(setpoint, _states, true, !was_in_failsafe); - } - - // Check if altitude, climbrate or thrust in D-direction are valid -> trigger failsafe if none - // of these setpoints are valid - if (!PX4_ISFINITE(setpoint.z) && !PX4_ISFINITE(setpoint.vz) && !PX4_ISFINITE(setpoint.thrust[2])) { - failsafe(setpoint, _states, true, !was_in_failsafe); - } } // publish trajectory setpoint @@ -591,10 +571,16 @@ MulticopterPositionControl::Run() constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now); constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up); - if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { - // we are not flying yet and need to avoid any corrections + // make sure we don't correct when not taken off yet or landed again + // since we lower the thrust it helps to decide if we are actually landed or not + const bool not_taken_off = _takeoff.getTakeoffState() <= TakeoffState::ready_for_takeoff; + const bool in_flight = _takeoff.getTakeoffState() >= TakeoffState::flight; + const bool ground_detected = _vehicle_land_detected.ground_contact || _vehicle_land_detected.maybe_landed; + + if (not_taken_off || (in_flight && ground_detected)) { + // override setpoints with level, idle thrust reset_setpoint_to_nan(setpoint); - setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; + Vector3f(0.f, 0.f, 100.f).copyTo(setpoint.acceleration); // set yaw-sp to current yaw // TODO: we need a clean way to disable yaw control setpoint.yaw = _states.yaw; @@ -605,7 +591,7 @@ MulticopterPositionControl::Run() _flight_tasks.reActivate(); } - if (_takeoff.getTakeoffState() < TakeoffState::flight && !PX4_ISFINITE(setpoint.thrust[2])) { + if (_takeoff.getTakeoffState() < TakeoffState::flight) { constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get()); } @@ -616,61 +602,39 @@ MulticopterPositionControl::Run() // Run position control _control.setState(_states); + _control.setInputSetpoint(setpoint); _control.setConstraints(constraints); - if (!_control.setInputSetpoint(setpoint)) { + if (!_control.update(_dt)) { warn_rate_limited("PositionControl: invalid setpoints"); failsafe(setpoint, _states, true, !was_in_failsafe); _control.setInputSetpoint(setpoint); - constraints = FlightTask::empty_constraints; + _control.setConstraints(FlightTask::empty_constraints); + _control.update(_dt); } - _control.update(_dt); - - // Fill local position, velocity and thrust setpoint. - // This message contains setpoints where each type of setpoint is either the input to the PositionController - // or was generated by the PositionController and therefore corresponds to the PositioControl internal states (states that were generated by P-PID). - // Example: - // If the desired setpoint is position-setpoint, _local_pos_sp will contain - // position-, velocity- and thrust-setpoint where the velocity- and thrust-setpoint were generated by the PositionControlller. - // If the desired setpoint has a velocity-setpoint only, then _local_pos_sp will contain valid velocity- and thrust-setpoint, but the position-setpoint - // will remain NAN. Given that the PositionController cannot generate a position-setpoint, this type of setpoint is always equal to the input to the - // PositionController. + // Get position control output vehicle_local_position_setpoint_s local_pos_sp{}; local_pos_sp.timestamp = time_stamp_now; _control.getLocalPositionSetpoint(local_pos_sp); - // Temporary setpoint message adjustments while PositionControl class is still keeping involved internal setpoints - local_pos_sp.x = setpoint.x; - local_pos_sp.y = setpoint.y; - local_pos_sp.z = setpoint.z; + vehicle_attitude_setpoint_s attitude_setpoint{}; + attitude_setpoint.timestamp = time_stamp_now; + _control.getAttitudeSetpoint(attitude_setpoint); // Publish local position setpoint // This message will be used by other modules (such as Landdetector) to determine vehicle intention. _local_pos_sp_pub.publish(local_pos_sp); + // Publish attitude setpoint + // Note: Only publish attitude setpoint when it's not coming from another source like + // stabilized mode direct attitude setpoint generation or offboard attitude commands. + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + // Inform FlightTask about the input and output of the velocity controller // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock) _flight_tasks.updateVelocityControllerIO(Vector3f(local_pos_sp.vx, local_pos_sp.vy, local_pos_sp.vz), Vector3f(local_pos_sp.thrust)); - vehicle_attitude_setpoint_s attitude_setpoint{}; - attitude_setpoint.timestamp = time_stamp_now; - _control.getAttitudeSetpoint(attitude_setpoint); - - // Part of landing logic: if ground-contact/maybe landed was detected, turn off - // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic. - // Note: only adust thrust output if there was not thrust-setpoint demand in D-direction. - if (_takeoff.getTakeoffState() > TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { - limit_thrust_during_landing(attitude_setpoint); - } - - // publish attitude setpoint - // It's important to publish also when disarmed otheriwse the attitude setpoint stays uninitialized. - // Not publishing when not running a flight task - // in stabilized mode attitude setpoints get ignored - // in offboard with attitude setpoints they come from MAVLink directly - _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); - _wv_dcm_z_sp_prev = Quatf(attitude_setpoint.q_d).dcm_z(); // if there's any change in landing gear setpoint publish it @@ -831,6 +795,11 @@ MulticopterPositionControl::start_flight_task() error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmoothVel); break; + case 4: + error = _flight_tasks.switchTask(FlightTaskIndex::ManualAcceleration); + break; + + default: error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition); break; @@ -904,21 +873,6 @@ MulticopterPositionControl::start_flight_task() } } -void -MulticopterPositionControl::limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint) -{ - if (_vehicle_land_detected.ground_contact - || _vehicle_land_detected.maybe_landed) { - // we set the collective thrust to zero, this will help to decide if we are actually landed or not - setpoint.thrust_body[2] = 0.f; - // go level to avoid corrections but keep the heading we have - Quatf(AxisAngle(0, 0, _states.yaw)).copyTo(setpoint.q_d); - setpoint.yaw_sp_move_rate = 0.f; - // prevent any position control integrator windup - _control.resetIntegral(); - } -} - void MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force, bool warn) @@ -946,6 +900,7 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint } else { // descend with land speed since we can't stop + setpoint.acceleration[0] = setpoint.acceleration[1] = 0.0f; setpoint.thrust[0] = setpoint.thrust[1] = 0.f; setpoint.vz = _param_mpc_land_speed.get(); @@ -963,7 +918,8 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint } else { // emergency descend with a bit below hover thrust setpoint.vz = NAN; - setpoint.thrust[2] = _param_mpc_thr_hover.get() * .8f; + setpoint.thrust[2] = -_param_mpc_thr_hover.get() * .8f; + Vector3f(0, 0, 0.3).copyTo(setpoint.acceleration); if (warn) { PX4_WARN("Failsafe: blind descend"); diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 4f9e153d4577..d8e9a37ed20c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -452,7 +452,7 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.0f); /** * Acceleration for auto and for manual * - * Note: In manual, this parameter is only used in MPC_POS_MODE 1. + * Note: In manual, this parameter is only used in MPC_POS_MODE 1, 4. * * @unit m/s/s * @min 2.0 diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index f5bdcde8b36e..19405d42cb9c 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -198,5 +198,3 @@ PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f); * @group Mission */ PARAM_DEFINE_INT32(NAV_FORCE_VT, 1); - -