From 0b8f092d2b93ac0d1b6c2a6202ad5bef327afeba Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 26 Oct 2020 14:40:15 +0100 Subject: [PATCH] Add acceleration stick mapping for position mode --- src/lib/CMakeLists.txt | 1 + src/lib/flight_tasks/CMakeLists.txt | 3 +- .../tasks/ManualAcceleration/CMakeLists.txt | 39 +++++ .../FlightTaskManualAcceleration.cpp | 113 +++++++++++++ .../FlightTaskManualAcceleration.hpp | 64 ++++++++ .../flight_tasks/tasks/Utility/CMakeLists.txt | 6 +- .../tasks/Utility/StickAccelerationXY.cpp | 151 ++++++++++++++++++ .../tasks/Utility/StickAccelerationXY.hpp | 84 ++++++++++ .../flight_tasks/tasks/Utility/StickYaw.cpp | 66 ++++++++ .../flight_tasks/tasks/Utility/StickYaw.hpp | 68 ++++++++ src/lib/flight_tasks/tasks/Utility/Sticks.cpp | 17 ++ src/lib/flight_tasks/tasks/Utility/Sticks.hpp | 15 ++ src/lib/slew_rate/CMakeLists.txt | 44 +++++ src/lib/slew_rate/SlewRate.hpp | 85 ++++++++++ src/lib/slew_rate/SlewRateTest.cpp | 72 +++++++++ src/lib/slew_rate/dummy.cpp | 0 .../MulticopterPositionControl.cpp | 4 + .../mc_pos_control/mc_pos_control_params.c | 11 +- 18 files changed, 837 insertions(+), 6 deletions(-) create mode 100644 src/lib/flight_tasks/tasks/ManualAcceleration/CMakeLists.txt create mode 100644 src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp create mode 100644 src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp create mode 100644 src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.cpp create mode 100644 src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.hpp create mode 100644 src/lib/flight_tasks/tasks/Utility/StickYaw.cpp create mode 100644 src/lib/flight_tasks/tasks/Utility/StickYaw.hpp create mode 100644 src/lib/slew_rate/CMakeLists.txt create mode 100644 src/lib/slew_rate/SlewRate.hpp create mode 100644 src/lib/slew_rate/SlewRateTest.cpp create mode 100644 src/lib/slew_rate/dummy.cpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index a17798db07..d963024682 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -59,6 +59,7 @@ add_subdirectory(perf) add_subdirectory(pid) add_subdirectory(rc) add_subdirectory(sensor_calibration) +add_subdirectory(slew_rate) add_subdirectory(systemlib) add_subdirectory(tecs) add_subdirectory(terrain_estimation) diff --git a/src/lib/flight_tasks/CMakeLists.txt b/src/lib/flight_tasks/CMakeLists.txt index 28f4a4f7a8..d79705f231 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 - 2020 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 @@ -63,6 +63,7 @@ list(APPEND flight_tasks_all Failsafe Descend Transition + ManualAcceleration ${flight_tasks_to_add} ) 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 0000000000..5f445d595b --- /dev/null +++ b/src/lib/flight_tasks/tasks/ManualAcceleration/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2020 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 FlightTaskManualAltitudeSmoothVel 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 0000000000..8e3e5322f3 --- /dev/null +++ b/src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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" + +using namespace matrix; + +FlightTaskManualAcceleration::FlightTaskManualAcceleration() : + _stick_acceleration_xy(this) +{}; + +bool FlightTaskManualAcceleration::activate(const vehicle_local_position_setpoint_s &last_setpoint) +{ + bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint); + + if (PX4_ISFINITE(last_setpoint.vx)) { + _velocity_setpoint.xy() = Vector2f(last_setpoint.vx, last_setpoint.vy); + + } else { + _velocity_setpoint.xy() = Vector2f(_velocity); + } + + if (PX4_ISFINITE(last_setpoint.acceleration[0])) { + _stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration[0], last_setpoint.acceleration[1])); + } + + return ret; +} + +bool FlightTaskManualAcceleration::update() +{ + bool ret = FlightTaskManualAltitudeSmoothVel::update(); + + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, + _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime); + _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, _position, + _deltatime); + _stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint); + + _constraints.want_takeoff = _checkTakeoff(); + return ret; +} + +void FlightTaskManualAcceleration::_ekfResetHandlerPositionXY() +{ + if (PX4_ISFINITE(_position_setpoint(0))) { + _position_setpoint(0) = _position(0); + _position_setpoint(1) = _position(1); + } +} + +void FlightTaskManualAcceleration::_ekfResetHandlerVelocityXY() +{ + if (PX4_ISFINITE(_velocity_setpoint(0))) { + _velocity_setpoint(0) = _velocity(0); + _velocity_setpoint(1) = _velocity(1); + } +} + +void FlightTaskManualAcceleration::_ekfResetHandlerPositionZ() +{ + if (PX4_ISFINITE(_position_setpoint(2))) { + _position_setpoint(2) = _position(2); + } +} + +void FlightTaskManualAcceleration::_ekfResetHandlerVelocityZ() +{ + if (PX4_ISFINITE(_velocity_setpoint(2))) { + _velocity_setpoint(2) = _velocity(2); + } +} + +void FlightTaskManualAcceleration::_ekfResetHandlerHeading(float delta_psi) +{ + 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 0000000000..2073de6593 --- /dev/null +++ b/src/lib/flight_tasks/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 "FlightTaskManualAltitudeSmoothVel.hpp" +#include "StickAccelerationXY.hpp" +#include "StickYaw.hpp" + +class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel +{ +public: + FlightTaskManualAcceleration(); + virtual ~FlightTaskManualAcceleration() = default; + bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override; + bool update() override; + +private: + StickAccelerationXY _stick_acceleration_xy; + StickYaw _stick_yaw; + + void _ekfResetHandlerPositionXY() override; + void _ekfResetHandlerVelocityXY() override; + void _ekfResetHandlerPositionZ() override; + void _ekfResetHandlerVelocityZ() override; + void _ekfResetHandlerHeading(float delta_psi) override; +}; diff --git a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt index 4a4de910a0..439e2aa837 100644 --- a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2018 PX4 Development Team. All rights reserved. +# Copyright (c) 2018 - 2020 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 @@ -39,9 +39,11 @@ px4_add_library(FlightTaskUtility ManualVelocitySmoothingXY.cpp ManualVelocitySmoothingZ.cpp Sticks.cpp + StickAccelerationXY.cpp + StickYaw.cpp ) -target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier) +target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier 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/StickAccelerationXY.cpp b/src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.cpp new file mode 100644 index 0000000000..285cbc79ca --- /dev/null +++ b/src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.cpp @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 spec{fic 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 StickAccelerationXY.cpp + */ + +#include "StickAccelerationXY.hpp" + +#include +#include "Sticks.hpp" + +using namespace matrix; + +StickAccelerationXY::StickAccelerationXY(ModuleParams *parent) : + ModuleParams(parent) +{ + _brake_boost_filter.reset(1.f); + resetPosition(); +} + +void StickAccelerationXY::resetPosition() +{ + _position.setNaN(); +} + +void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity) +{ + _velocity = velocity; +} + +void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration) +{ + _acceleration_slew_rate_x.setForcedValue(acceleration(0)); + _acceleration_slew_rate_x.setForcedValue(acceleration(1)); +} + +void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos, + const float dt) +{ + // maximum commanded acceleration and velocity + Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get()); + Vector2f velocity_scale(_param_mpc_vel_manual.get(), _param_mpc_vel_manual.get()); + + acceleration_scale *= 2.f; // because of drag the average aceleration is half + + // Map stick input to acceleration + Sticks::limitStickUnitLengthXY(stick_xy); + Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp); + _acceleration = stick_xy.emult(acceleration_scale); + applyFeasibilityLimit(_acceleration, dt); + + // Add drag to limit speed and brake again + _acceleration -= calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity); + + applyTiltLimit(_acceleration); + + // Generate velocity setpoint by forward integrating commanded acceleration + _velocity += Vector2f(_acceleration) * dt; + + lockPosition(_velocity, pos, dt, _position); +} + +void StickAccelerationXY::getSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, Vector3f &acc_sp) +{ + pos_sp.xy() = _position; + vel_sp.xy() = _velocity; + acc_sp.xy() = _acceleration; +} + +void StickAccelerationXY::applyFeasibilityLimit(Vector2f &acceleration, const float dt) +{ + // Apply jerk limit - acceleration slew rate + _acceleration_slew_rate_x.setSlewRate(_param_mpc_jerk_max.get()); + _acceleration_slew_rate_y.setSlewRate(_param_mpc_jerk_max.get()); + acceleration(0) = _acceleration_slew_rate_x.update(acceleration(0), dt); + acceleration(1) = _acceleration_slew_rate_y.update(acceleration(1), dt); +} + +Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const float dt, const Vector2f &stick_xy, + const Vector2f &vel_sp) +{ + _brake_boost_filter.setParameters(dt, .8f); + + if (stick_xy.norm_squared() < FLT_EPSILON) { + _brake_boost_filter.update(2.f); + + } else { + _brake_boost_filter.update(1.f); + } + + drag_coefficient *= _brake_boost_filter.getState(); + + return drag_coefficient.emult(vel_sp); +} + +void StickAccelerationXY::applyTiltLimit(Vector2f &acceleration) +{ + // Check if acceleration would exceed the tilt limit + const float acc = acceleration.length(); + const float acc_tilt_max = tanf(M_DEG_TO_RAD_F * _param_mpc_tiltmax_air.get()) * CONSTANTS_ONE_G; + + if (acc > acc_tilt_max) { + acceleration *= acc_tilt_max / acc; + } +} + +void StickAccelerationXY::lockPosition(const Vector2f &vel_sp, const Vector3f &pos, const float dt, Vector2f &pos_sp) +{ + if (vel_sp.norm_squared() < FLT_EPSILON) { + if (!PX4_ISFINITE(pos_sp(0))) { + pos_sp = Vector2f(pos); + } + + pos_sp += vel_sp * dt; + + } else { + pos_sp.setNaN(); + + } +} diff --git a/src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.hpp b/src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.hpp new file mode 100644 index 0000000000..ccdff506ff --- /dev/null +++ b/src/lib/flight_tasks/tasks/Utility/StickAccelerationXY.hpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 StickAccelerationXY.hpp + * @brief Generate horizontal position, velocity and acceleration from stick input + * @author Matthias Grob + */ + +#pragma once + +#include +#include // TODO add this include to AlphaFilter since it's used there +#include +#include + +#include "SlewRate.hpp" + +class StickAccelerationXY : public ModuleParams +{ +public: + StickAccelerationXY(ModuleParams *parent); + ~StickAccelerationXY() = default; + + void resetPosition(); + void resetVelocity(const matrix::Vector2f &velocity); + void resetAcceleration(const matrix::Vector2f &acceleration); + void generateSetpoints(matrix::Vector2f stick_xy, const float yaw, const float yaw_sp, const matrix::Vector3f &pos, + const float dt); + void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp); + +private: + void applyFeasibilityLimit(matrix::Vector2f &acceleration, const float dt); + matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy, + const matrix::Vector2f &vel_sp); + void applyTiltLimit(matrix::Vector2f &acceleration); + void lockPosition(const matrix::Vector2f &vel_sp, const matrix::Vector3f &pos, const float dt, + matrix::Vector2f &pos_sp); + + SlewRate _acceleration_slew_rate_x; + SlewRate _acceleration_slew_rate_y; + AlphaFilter _brake_boost_filter; + + matrix::Vector2f _position; + matrix::Vector2f _velocity; + matrix::Vector2f _acceleration; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_tiltmax_air + ) +}; diff --git a/src/lib/flight_tasks/tasks/Utility/StickYaw.cpp b/src/lib/flight_tasks/tasks/Utility/StickYaw.cpp new file mode 100644 index 0000000000..5e0217045f --- /dev/null +++ b/src/lib/flight_tasks/tasks/Utility/StickYaw.cpp @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 StickYaw.cpp + */ + +#include "StickYaw.hpp" + +#include + +void StickYaw::generateYawSetpoint(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); +} + +float StickYaw::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; + } + } +} diff --git a/src/lib/flight_tasks/tasks/Utility/StickYaw.hpp b/src/lib/flight_tasks/tasks/Utility/StickYaw.hpp new file mode 100644 index 0000000000..3fde9aa1ef --- /dev/null +++ b/src/lib/flight_tasks/tasks/Utility/StickYaw.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 StickYaw.hpp + * @brief Generate yaw and angular yawspeed setpoints from stick input + * @author Matthias Grob + */ + +#pragma once + +#include "SlewRate.hpp" + +class StickYaw +{ +public: + StickYaw() = default; + ~StickYaw() = default; + + void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float desired_yawspeed, const float yaw, + const float deltatime); + +private: + SlewRate _yawspeed_slew_rate; + + /** + * 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); +}; diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.cpp b/src/lib/flight_tasks/tasks/Utility/Sticks.cpp index 8ca4fa28ef..e489332590 100644 --- a/src/lib/flight_tasks/tasks/Utility/Sticks.cpp +++ b/src/lib/flight_tasks/tasks/Utility/Sticks.cpp @@ -38,6 +38,7 @@ #include "Sticks.hpp" using namespace time_literals; +using namespace matrix; Sticks::Sticks(ModuleParams *parent) : ModuleParams(parent) @@ -108,3 +109,19 @@ void Sticks::setGearAccordingToSwitch(landing_gear_s &gear) _gear_switch_old = gear_switch; } } + +void Sticks::limitStickUnitLengthXY(Vector2f &v) +{ + const float vl = v.length(); + + if (vl > 1.0f) { + v /= vl; + } +} + +void Sticks::rotateIntoHeadingFrameXY(Vector2f &v, const float yaw, const float yaw_setpoint) +{ + 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); +} diff --git a/src/lib/flight_tasks/tasks/Utility/Sticks.hpp b/src/lib/flight_tasks/tasks/Utility/Sticks.hpp index a6f40c9acb..94e9334a44 100644 --- a/src/lib/flight_tasks/tasks/Utility/Sticks.hpp +++ b/src/lib/flight_tasks/tasks/Utility/Sticks.hpp @@ -59,6 +59,21 @@ public: const matrix::Vector &getPosition() { return _positions; }; const matrix::Vector &getPositionExpo() { return _positions_expo; }; + + /** + * 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 + */ + static void limitStickUnitLengthXY(matrix::Vector2f &v); + + /** + * 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 + */ + static void rotateIntoHeadingFrameXY(matrix::Vector2f &v, const float yaw, const float yaw_setpoint); + private: bool _input_available = false; matrix::Vector _positions; ///< unmodified manual stick inputs diff --git a/src/lib/slew_rate/CMakeLists.txt b/src/lib/slew_rate/CMakeLists.txt new file mode 100644 index 0000000000..fb36f67a25 --- /dev/null +++ b/src/lib/slew_rate/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2020 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/slew_rate/SlewRate.hpp b/src/lib/slew_rate/SlewRate.hpp new file mode 100644 index 0000000000..6297258ab8 --- /dev/null +++ b/src/lib/slew_rate/SlewRate.hpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 + +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 = math::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 +}; diff --git a/src/lib/slew_rate/SlewRateTest.cpp b/src/lib/slew_rate/SlewRateTest.cpp new file mode 100644 index 0000000000..6e03d9bef2 --- /dev/null +++ b/src/lib/slew_rate/SlewRateTest.cpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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); + } +} diff --git a/src/lib/slew_rate/dummy.cpp b/src/lib/slew_rate/dummy.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 7abc8be07f..647554002a 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -597,6 +597,10 @@ void 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; 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 1bf49a337e..59d42627f4 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -463,7 +463,10 @@ PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); /** * Maximum horizontal acceleration for auto mode and for manual mode * - * Maximum deceleration for MPC_POS_MODE 1. Maximum acceleration and deceleration for MPC_POS_MODE 3. + * MPC_POS_MODE + * 1 just deceleration + * 3 acceleration and deceleration + * 4 just acceleration * * @unit m/s^2 * @min 2.0 @@ -536,7 +539,7 @@ PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.0f); * * Setting this to the maximum value essentially disables the limit. * - * Note: This is only used when MPC_POS_MODE is set to a smoothing mode 1 or 3. + * Note: This is only used when MPC_POS_MODE is set to a smoothing mode 1, 3 or 4. * * @unit m/s^3 * @min 0.5 @@ -727,13 +730,15 @@ PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f); * 1 Smooth position control with maximum acceleration and jerk limits based on slew-rates. * 3 Smooth position control with maximum acceleration and jerk limits based on * jerk optimized trajectory generator (different algorithm than 1). + * 4 Smooth position control where sticks map to acceleration and there's a virtual brake drag * * @value 0 Simple position control * @value 1 Smooth position control * @value 3 Smooth position control (Jerk optimized) + * @value 4 Acceleration based input * @group Multicopter Position Control */ -PARAM_DEFINE_INT32(MPC_POS_MODE, 3); +PARAM_DEFINE_INT32(MPC_POS_MODE, 4); /** * Enforced delay between arming and takeoff