From b82e398d8725ec275bc8980022c85611c9d88ded Mon Sep 17 00:00:00 2001 From: Miron Kolodziejczyk Date: Thu, 27 Jan 2022 08:21:58 +0100 Subject: [PATCH] init commit --- src/lib/lqr/CMakeLists.txt | 38 + src/lib/lqr/lqr.hpp | 102 +++ src/modules/mc_lqr_control/CMakeLists.txt | 50 ++ src/modules/mc_lqr_control/Kconfig | 5 + .../mc_lqr_control/LqrControl/CMakeLists.txt | 39 + .../mc_lqr_control/LqrControl/LqrControl.cpp | 315 +++++++ .../mc_lqr_control/LqrControl/LqrControl.hpp | 177 ++++ .../mc_lqr_control/MulticopterLqrControl.cpp | 698 +++++++++++++++ .../mc_lqr_control/MulticopterLqrControl.hpp | 253 ++++++ .../mc_lqr_control/Takeoff/CMakeLists.txt | 41 + .../mc_lqr_control/Takeoff/Takeoff.cpp | 134 +++ .../mc_lqr_control/Takeoff/Takeoff.hpp | 102 +++ .../mc_lqr_control/Takeoff/TakeoffTest.cpp | 79 ++ .../mc_lqr_control/mc_lqr_control_params.c | 836 ++++++++++++++++++ 14 files changed, 2869 insertions(+) create mode 100644 src/lib/lqr/CMakeLists.txt create mode 100644 src/lib/lqr/lqr.hpp create mode 100644 src/modules/mc_lqr_control/CMakeLists.txt create mode 100644 src/modules/mc_lqr_control/Kconfig create mode 100644 src/modules/mc_lqr_control/LqrControl/CMakeLists.txt create mode 100644 src/modules/mc_lqr_control/LqrControl/LqrControl.cpp create mode 100644 src/modules/mc_lqr_control/LqrControl/LqrControl.hpp create mode 100644 src/modules/mc_lqr_control/MulticopterLqrControl.cpp create mode 100644 src/modules/mc_lqr_control/MulticopterLqrControl.hpp create mode 100644 src/modules/mc_lqr_control/Takeoff/CMakeLists.txt create mode 100644 src/modules/mc_lqr_control/Takeoff/Takeoff.cpp create mode 100644 src/modules/mc_lqr_control/Takeoff/Takeoff.hpp create mode 100644 src/modules/mc_lqr_control/Takeoff/TakeoffTest.cpp create mode 100644 src/modules/mc_lqr_control/mc_lqr_control_params.c diff --git a/src/lib/lqr/CMakeLists.txt b/src/lib/lqr/CMakeLists.txt new file mode 100644 index 000000000000..49b5cbfa7a4f --- /dev/null +++ b/src/lib/lqr/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# 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 +# 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(lqr + lqr.hpp +) + +# target_link_libraries(lqr PRIVATE geo) diff --git a/src/lib/lqr/lqr.hpp b/src/lib/lqr/lqr.hpp new file mode 100644 index 000000000000..e2a4ba03bacf --- /dev/null +++ b/src/lib/lqr/lqr.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +#ifndef LQR_H_ +#define LQR_H_ + +#include +#include + +#include +#include +#include +#include + +using namespace matrix; + +//TODO should be in Matrix utils? +// see https://github.com/PX4/PX4-Autopilot/blob/0607982b234f4ddc0f8037829fcb051d8cb34b98/src/lib/matrix/matrix/Matrix.hpp#L803-L821 +// why it is under SUPPORT_STDIOSTREAM? +template +void read_matrix(std::string file_path, Matrix &matrix_out) { + + static std::ifstream infile; + infile.open(file_path); + + if (infile.is_open() == true) { + for (unsigned row = 0; row < M; row++) { + std::string line; + getline(infile, line); + std::stringstream this_stream(line); + for (unsigned column = 0 ; column < N; column++) { + this_stream >> matrix_out(row, column); + } + } + infile.close(); + } else + PX4_ERR("K.txt not opened"); +} + +template +class Lqr +{ +public: + + Lqr() {set_K("K.txt");} + ~Lqr() = default; + + bool update(const Vector &x, const Vector &x_sp, Vector &u) + { + Vector delta_x = x_sp - x; + u = _K * delta_x; + + return true; + } + + void set_K(std::string file_path) { + read_matrix(file_path, _K); + } + +private: + // Matrix _A; + // Matrix _B; + Matrix _K; + + // Vector _x; + // Vector _u; + // Vector _x_sp; + // Vector _u_sp; +}; + + +#endif /* LQR_H_ */ diff --git a/src/modules/mc_lqr_control/CMakeLists.txt b/src/modules/mc_lqr_control/CMakeLists.txt new file mode 100644 index 000000000000..ec82bcc0b7d2 --- /dev/null +++ b/src/modules/mc_lqr_control/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2015-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. +# +############################################################################ + +add_subdirectory(LqrControl) +add_subdirectory(Takeoff) + +px4_add_module( + MODULE modules__mc_lqr_control + MAIN mc_lqr_control + COMPILE_FLAGS + SRCS + MulticopterLqrControl.cpp + MulticopterLqrControl.hpp + DEPENDS + LqrControl + Takeoff + controllib + geo + SlewRate + ) diff --git a/src/modules/mc_lqr_control/Kconfig b/src/modules/mc_lqr_control/Kconfig new file mode 100644 index 000000000000..538466bd3338 --- /dev/null +++ b/src/modules/mc_lqr_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_MC_LQR_CONTROL + bool "mc_lqr_control" + default n + ---help--- + Enable support for mc_lqr_control diff --git a/src/modules/mc_lqr_control/LqrControl/CMakeLists.txt b/src/modules/mc_lqr_control/LqrControl/CMakeLists.txt new file mode 100644 index 000000000000..6986549e9cd6 --- /dev/null +++ b/src/modules/mc_lqr_control/LqrControl/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(LqrControl + LqrControl.cpp + LqrControl.hpp +) +target_include_directories(LqrControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(LqrControl PUBLIC lqr) diff --git a/src/modules/mc_lqr_control/LqrControl/LqrControl.cpp b/src/modules/mc_lqr_control/LqrControl/LqrControl.cpp new file mode 100644 index 000000000000..dfe3e23f8277 --- /dev/null +++ b/src/modules/mc_lqr_control/LqrControl/LqrControl.cpp @@ -0,0 +1,315 @@ +/**************************************************************************** + * + * Copyright (c) 2018 - 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 LqrControl.cpp + */ + +#include "LqrControl.hpp" +#include +#include +#include +#include + +using namespace matrix; + +// void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +// { +// _gain_vel_p = P; +// _gain_vel_i = I; +// _gain_vel_d = D; +// } + +// void PositionControl::setVelocityLimits(const float vel_horizontal, const float vel_up, const float vel_down) +// { +// _lim_vel_horizontal = vel_horizontal; +// _lim_vel_up = vel_up; +// _lim_vel_down = vel_down; +// } + +void LqrControl::setThrustLimits(const float min, const float max) +{ + // make sure there's always enough thrust vector length to infer the attitude + _lim_thr_min = math::max(min, 10e-4f); + _lim_thr_max = max; +} + +void LqrControl::setHorizontalThrustMargin(const float margin) +{ + _lim_thr_xy_margin = margin; +} + +void LqrControl::updateHoverThrust(const float hover_thrust_new) +{ + // Given that the equation for thrust is T = a_sp * Th / g - Th + // with a_sp = desired acceleration, Th = hover thrust and g = gravity constant, + // we want to find the acceleration that needs to be added to the integrator in order obtain + // the same thrust after replacing the current hover thrust by the new one. + // T' = T => a_sp' * Th' / g - Th' = a_sp * Th / g - Th + // so a_sp' = (a_sp - g) * Th / Th' + g + // we can then add a_sp' - a_sp to the current integrator to absorb the effect of changing Th by Th' + if (hover_thrust_new > FLT_EPSILON) { + // _vel_int(2) += (_acc_sp(2) - CONSTANTS_ONE_G) * _hover_thrust / hover_thrust_new + CONSTANTS_ONE_G - _acc_sp(2); + setHoverThrust(hover_thrust_new); + } +} + +void LqrControl::setState(const PositionControlStates &states) +{ + // PositionControlStates -> Vector + + //TODO do better copying + + _x = Vector(); + _x(0) = states.velocity(0); + _x(1) = states.position(0); + _x(2) = states.velocity(1); + _x(3) = states.position(1); + _x(4) = states.velocity(2); + _x(5) = states.position(2); + _x(6) = states.angular_velocity(0); + _x(7) = states.attitude(0); + _x(8) = states.angular_velocity(1); + _x(9) = states.attitude(1); + _x(10) = states.angular_velocity(2); + _x(11) = states.attitude(2); +} + +void LqrControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint) +{ + // vehicle_local_position_setpoint_s -> Vector + + // _x_sp = Vector(); + _x_sp = zeros(); + // _x_sp(0) = setpoint.x; + // _x_sp(1) = setpoint.y; + // _x_sp(2) = setpoint.z; + + _x_sp(1) = setpoint.x; + _x_sp(3) = setpoint.y; + _x_sp(5) = setpoint.z; + + _x_sp(0) = setpoint.vx; + _x_sp(2) = setpoint.vy; + _x_sp(4) = setpoint.vz; + + // _x_sp(11) = setpoint.yaw; + _x_sp(11) = PX4_ISFINITE(setpoint.yaw) ? setpoint.yaw : _x(11); + + // PX4_INFO("setpoint.x: %f", double(setpoint.x)); + // PX4_INFO("setpoint.yaw: %f", double(setpoint.yaw)); + +} + +bool LqrControl::update(const float dt) +{ + // // x and y input setpoints always have to come in pairs + // const bool valid = (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1))) + // && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1))) + // && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1))); + + // _positionControl(); + // _velocityControl(dt); + + // Vector _u; + lqr.update(_x, _x_sp, _u); + + //TODO actuator_output_s? + + // normalize + _u(0) = (_u(0)+9.81f*1.5f+7.50f)/28.27f; + _u(1) = float(_u(1)/1.84f); + _u(2) = float(_u(2)/3.11f); + _u(3) = float(_u(3)/1.70f); + + _u(0) = math::constrain(_u(0), 0.0f, 1.0f); + _u(1) = math::constrain(_u(1), -1.0f, 1.0f); + _u(2) = math::constrain(_u(2), -1.0f, 1.0f); + _u(3) = math::constrain(_u(3), -1.0f, 1.0f); + + // _u(0) = 0.0; + // _u(1) = 0.0; + // _u(2) = 0.0; + // _u(3) = 0.0; + + + // _yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f; + // _yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control + + // return valid && _updateSuccessful(); + + return _updateSuccessful(); +} + +// void LqrControl::_positionControl() +// { +// // P-position controller +// Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); +// // Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence +// ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position); +// // make sure there are no NAN elements for further reference while constraining +// ControlMath::setZeroIfNanVector3f(vel_sp_position); + +// // Constrain horizontal velocity by prioritizing the velocity component along the +// // the desired position setpoint over the feed-forward term. +// _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), -_lim_vel_up, _lim_vel_down); +// } + +// void LqrControl::_velocityControl(const float dt) +// { +// // 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); + +// // No control input from setpoints or corresponding states which are NAN +// ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); + +// _accelerationControl(); + +// // Integrator anti-windup in vertical direction +// if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) || +// (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) { +// vel_error(2) = 0.f; +// } + +// // Prioritize vertical control while keeping a horizontal margin +// const Vector2f thrust_sp_xy(_thr_sp); +// const float thrust_sp_xy_norm = thrust_sp_xy.norm(); +// const float thrust_max_squared = math::sq(_lim_thr_max); + +// // Determine how much vertical thrust is left keeping horizontal margin +// const float allocated_horizontal_thrust = math::min(thrust_sp_xy_norm, _lim_thr_xy_margin); +// const float thrust_z_max_squared = thrust_max_squared - math::sq(allocated_horizontal_thrust); + +// // Saturate maximal vertical thrust +// _thr_sp(2) = math::max(_thr_sp(2), -sqrtf(thrust_z_max_squared)); + +// // Determine how much horizontal thrust is left after prioritizing vertical control +// const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2)); +// float thrust_max_xy = 0; + +// if (thrust_max_xy_squared > 0) { +// thrust_max_xy = sqrtf(thrust_max_xy_squared); +// } + +// // Saturate thrust in horizontal direction +// if (thrust_sp_xy_norm > thrust_max_xy) { +// _thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy; +// } + +// // 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 Vector2f acc_sp_xy_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust); +// const float arw_gain = 2.f / _gain_vel_p(0); +// vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited)); + +// // Make sure integral doesn't get NAN +// ControlMath::setZeroIfNanVector3f(vel_error); +// // Update integral part of velocity control +// _vel_int += vel_error.emult(_gain_vel_i) * dt; + +// // limit thrust integral +// _vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2)); +// } + +// void LqrControl::_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), _lim_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_min); +// _thr_sp = body_z * collective_thrust; +// } + +bool LqrControl::_updateSuccessful() +{ + bool valid = true; + + // // For each controlled state the estimate has to be valid + // for (int i = 0; i <= 2; i++) { + // if (PX4_ISFINITE(_pos_sp(i))) { + // valid = valid && PX4_ISFINITE(_pos(i)); + // } + + // if (PX4_ISFINITE(_vel_sp(i))) { + // valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i)); + // } + // } + + // // There has to be a valid output accleration and thrust setpoint otherwise there was no + // // setpoint-state pair for each axis that can get controlled + // valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2)); + // valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2)); + return valid; +} + +void LqrControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const +{ + // local_position_setpoint.x = _pos_sp(0); + // local_position_setpoint.y = _pos_sp(1); + // local_position_setpoint.z = _pos_sp(2); + // local_position_setpoint.yaw = _yaw_sp; + // local_position_setpoint.yawspeed = _yawspeed_sp; + // local_position_setpoint.vx = _vel_sp(0); + // local_position_setpoint.vy = _vel_sp(1); + // local_position_setpoint.vz = _vel_sp(2); + // _acc_sp.copyTo(local_position_setpoint.acceleration); + // _thr_sp.copyTo(local_position_setpoint.thrust); +} + +// void LqrControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const +// { +// ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint); +// attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; +// } + +void LqrControl::getActuatorControls(actuator_controls_s &actuator_controls) const +{ + // actuator_controls.control[0] = _u(0); + // actuator_controls.control[1] = _u(1); + // actuator_controls.control[2] = _u(2); + // actuator_controls.control[3] = _u(3); + + actuator_controls.control[0] = _u(1); // roll + actuator_controls.control[1] = _u(2); // pitch + actuator_controls.control[2] = _u(3); // yaw + actuator_controls.control[3] = _u(0); // throttle + // actuator_controls.control[3] = 0.8; +} + diff --git a/src/modules/mc_lqr_control/LqrControl/LqrControl.hpp b/src/modules/mc_lqr_control/LqrControl/LqrControl.hpp new file mode 100644 index 000000000000..654e942f516b --- /dev/null +++ b/src/modules/mc_lqr_control/LqrControl/LqrControl.hpp @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (c) 2018 - 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 LqrControl.hpp + * + * A cascaded position controller for position/velocity control only. + */ + +#pragma once + +#include +#include +#include +// #include +#include + +#include + +using namespace matrix; + + +struct PositionControlStates { + matrix::Vector3f position; + matrix::Vector3f velocity; + matrix::Vector3f attitude; + matrix::Vector3f angular_velocity; +}; + + +class LqrControl +{ +public: + + LqrControl() = default; + ~LqrControl() = default; + + // /** + // * Set the maximum velocity to execute with feed forward and position control + // * @param vel_horizontal horizontal velocity limit + // * @param vel_up upwards velocity limit + // * @param vel_down downwards velocity limit + // */ + // void setVelocityLimits(const float vel_horizontal, const float vel_up, float vel_down); + + /** + * Set the minimum and maximum collective normalized thrust [0,1] that can be output by the controller + * @param min minimum thrust e.g. 0.1 or 0 + * @param max maximum thrust e.g. 0.9 or 1 + */ + void setThrustLimits(const float min, const float max); + + /** + * Set margin that is kept for horizontal control when prioritizing vertical thrust + * @param margin of normalized thrust that is kept for horizontal control e.g. 0.3 + */ + void setHorizontalThrustMargin(const float margin); + + // /** + // * Set the maximum tilt angle in radians the output attitude is allowed to have + // * @param tilt angle in radians from level orientation + // */ + // void setTiltLimit(const float tilt) { _lim_tilt = tilt; } + + /** + * Set the normalized hover thrust + * @param thrust [0.1, 0.9] with which the vehicle hovers not acelerating down or up with level orientation + */ + void setHoverThrust(const float hover_thrust) { _hover_thrust = math::constrain(hover_thrust, 0.1f, 0.9f); } + + /** + * Update the hover thrust without immediately affecting the output + * by adjusting the integrator. This prevents propagating the dynamics + * of the hover thrust signal directly to the output of the controller. + */ + void updateHoverThrust(const float hover_thrust_new); + + /** + * Pass the current vehicle state to the controller + * @param PositionControlStates structure + */ + void setState(const PositionControlStates &states); + + /** + * 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 + */ + void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint); + + /** + * Apply P-position and PID-velocity controller that updates the member + * thrust, yaw- and yawspeed-setpoints. + * @see _thr_sp + * @see _yaw_sp + * @see _yawspeed_sp + * @param dt time in seconds since last iteration + * @return true if update succeeded and output setpoint is executable, false if not + */ + bool update(const float dt); + + /** + * Get the controllers output local position setpoint + * These setpoints are the ones which were executed on including PID output and feed-forward. + * The acceleration or thrust setpoints can be used for attitude control. + * @param local_position_setpoint reference to struct to fill up + */ + void getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const; + + void getActuatorControls(actuator_controls_s &actuator_controls) const; + + +private: + bool _updateSuccessful(); + + Lqr<12, 4> lqr; + + // Limits + // float _lim_vel_horizontal{}; ///< Horizontal velocity limit with feed forward and position control + // float _lim_vel_up{}; ///< Upwards velocity limit with feed forward and position control + // float _lim_vel_down{}; ///< Downwards velocity limit with feed forward and position control + float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9 + float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1 + float _lim_thr_xy_margin{}; ///< Margin to keep for horizontal control when saturating prioritized vertical thrust + // float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have + + float _hover_thrust{}; ///< Thrust [0.1, 0.9] with which the vehicle hovers not accelerating down or up with level orientation + + Vector _x; + Vector _x_sp; + Vector _u; + + // // States + // matrix::Vector3f _pos; /**< current position */ + // matrix::Vector3f _vel; /**< current velocity */ + // matrix::Vector3f _vel_dot; /**< velocity derivative (replacement for acceleration estimate) */ + // matrix::Vector3f _vel_int; /**< integral term of the velocity controller */ + // float _yaw{}; /**< current heading */ + + // // Setpoints + // matrix::Vector3f _pos_sp; /**< desired position */ + // matrix::Vector3f _vel_sp; /**< desired velocity */ + // matrix::Vector3f _acc_sp; /**< desired acceleration */ + // matrix::Vector3f _thr_sp; /**< desired thrust */ + // float _yaw_sp{}; /**< desired heading */ + // float _yawspeed_sp{}; /** desired yaw-speed */ +}; diff --git a/src/modules/mc_lqr_control/MulticopterLqrControl.cpp b/src/modules/mc_lqr_control/MulticopterLqrControl.cpp new file mode 100644 index 000000000000..eca304b5ea73 --- /dev/null +++ b/src/modules/mc_lqr_control/MulticopterLqrControl.cpp @@ -0,0 +1,698 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 "MulticopterLqrControl.hpp" + +#include +#include +#include +#include + +using namespace matrix; + +MulticopterLqrControl::MulticopterLqrControl(bool vtol) : + SuperBlock(nullptr, "MPC"), + ModuleParams(nullptr), + // ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + // WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + // _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), + _vel_x_deriv(this, "VELD"), + _vel_y_deriv(this, "VELD"), + _vel_z_deriv(this, "VELD") +{ + parameters_update(true); + _failsafe_land_hysteresis.set_hysteresis_time_from(false, LOITER_TIME_BEFORE_DESCEND); + _tilt_limit_slew_rate.setSlewRate(.2f); + reset_setpoint_to_nan(_setpoint); + _takeoff_status_pub.advertise(); +} + +MulticopterLqrControl::~MulticopterLqrControl() +{ + perf_free(_cycle_perf); +} + +bool MulticopterLqrControl::init() +{ + // if (!_local_pos_sub.registerCallback()) { + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle_local_position callback registration failed!"); + return false; + } + + _time_stamp_last_loop = hrt_absolute_time(); + // ScheduleNow(); + + return true; +} + +int MulticopterLqrControl::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + ModuleParams::updateParams(); + SuperBlock::updateParams(); + + int num_changed = 0; + + if (_param_sys_vehicle_resp.get() >= 0.f) { + // make it less sensitive at the lower end + float responsiveness = _param_sys_vehicle_resp.get() * _param_sys_vehicle_resp.get(); + + num_changed += _param_mpc_acc_hor.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_hor_max.commit_no_notification(math::lerp(2.f, 15.f, responsiveness)); + num_changed += _param_mpc_man_y_max.commit_no_notification(math::lerp(80.f, 450.f, responsiveness)); + + if (responsiveness > 0.6f) { + num_changed += _param_mpc_man_y_tau.commit_no_notification(0.f); + + } else { + num_changed += _param_mpc_man_y_tau.commit_no_notification(math::lerp(0.5f, 0.f, responsiveness / 0.6f)); + } + + if (responsiveness < 0.5f) { + num_changed += _param_mpc_tiltmax_air.commit_no_notification(45.f); + + } else { + num_changed += _param_mpc_tiltmax_air.commit_no_notification(math::min(MAX_SAFE_TILT_DEG, math::lerp(45.f, 70.f, + (responsiveness - 0.5f) * 2.f))); + } + + num_changed += _param_mpc_acc_down_max.commit_no_notification(math::lerp(0.8f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_up_max.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_jerk_max.commit_no_notification(math::lerp(2.f, 50.f, responsiveness)); + num_changed += _param_mpc_jerk_auto.commit_no_notification(math::lerp(1.f, 25.f, responsiveness)); + } + + if (_param_mpc_xy_vel_all.get() >= 0.f) { + float xy_vel = _param_mpc_xy_vel_all.get(); + num_changed += _param_mpc_vel_manual.commit_no_notification(xy_vel); + num_changed += _param_mpc_xy_cruise.commit_no_notification(xy_vel); + num_changed += _param_mpc_xy_vel_max.commit_no_notification(xy_vel); + } + + if (_param_mpc_z_vel_all.get() >= 0.f) { + float z_vel = _param_mpc_z_vel_all.get(); + num_changed += _param_mpc_z_v_auto_up.commit_no_notification(z_vel); + num_changed += _param_mpc_z_vel_max_up.commit_no_notification(z_vel); + num_changed += _param_mpc_z_v_auto_dn.commit_no_notification(z_vel * 0.75f); + num_changed += _param_mpc_z_vel_max_dn.commit_no_notification(z_vel * 0.75f); + num_changed += _param_mpc_tko_speed.commit_no_notification(z_vel * 0.6f); + num_changed += _param_mpc_land_speed.commit_no_notification(z_vel * 0.5f); + } + + if (num_changed > 0) { + param_notify_changes(); + } + + if (_param_mpc_tiltmax_air.get() > MAX_SAFE_TILT_DEG) { + _param_mpc_tiltmax_air.set(MAX_SAFE_TILT_DEG); + _param_mpc_tiltmax_air.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Tilt constrained to safe value\t"); + /* EVENT + * @description MPC_TILTMAX_AIR is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_tilt_set"), events::Log::Warning, + "Maximum tilt limit has been constrained to a safe value", MAX_SAFE_TILT_DEG); + } + + if (_param_mpc_tiltmax_lnd.get() > _param_mpc_tiltmax_air.get()) { + _param_mpc_tiltmax_lnd.set(_param_mpc_tiltmax_air.get()); + _param_mpc_tiltmax_lnd.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Land tilt has been constrained by max tilt\t"); + /* EVENT + * @description MPC_TILTMAX_LND is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_land_tilt_set"), events::Log::Warning, + "Land tilt limit has been constrained by maximum tilt", _param_mpc_tiltmax_air.get()); + } + + // _control.setPositionGains(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(), _param_mpc_z_p.get())); + // _control.setVelocityGains( + // Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()), + // Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()), + // Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get())); + // _control.setHorizontalThrustMargin(_param_mpc_thr_xy_marg.get()); + + // Check that the design parameters are inside the absolute maximum constraints + if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) { + _param_mpc_xy_cruise.set(_param_mpc_xy_vel_max.get()); + _param_mpc_xy_cruise.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_XY_CRUISE is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_cruise_set"), events::Log::Warning, + "Cruise speed has been constrained by maximum speed", _param_mpc_xy_vel_max.get()); + } + + if (_param_mpc_vel_manual.get() > _param_mpc_xy_vel_max.get()) { + _param_mpc_vel_manual.set(_param_mpc_xy_vel_max.get()); + _param_mpc_vel_manual.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_VEL_MANUAL is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_man_vel_set"), events::Log::Warning, + "Manual speed has been constrained by maximum speed", _param_mpc_xy_vel_max.get()); + } + + if (_param_mpc_z_v_auto_up.get() > _param_mpc_z_vel_max_up.get()) { + _param_mpc_z_v_auto_up.set(_param_mpc_z_vel_max_up.get()); + _param_mpc_z_v_auto_up.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Ascent speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_Z_V_AUTO_UP is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_up_vel_set"), events::Log::Warning, + "Ascent speed has been constrained by max speed", _param_mpc_z_vel_max_up.get()); + } + + if (_param_mpc_z_v_auto_dn.get() > _param_mpc_z_vel_max_dn.get()) { + _param_mpc_z_v_auto_dn.set(_param_mpc_z_vel_max_dn.get()); + _param_mpc_z_v_auto_dn.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Descent speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_Z_V_AUTO_DN is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_down_vel_set"), events::Log::Warning, + "Descent speed has been constrained by max speed", _param_mpc_z_vel_max_dn.get()); + } + + if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() || + _param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) { + _param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(), + _param_mpc_thr_max.get())); + _param_mpc_thr_hover.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max\t"); + /* EVENT + * @description MPC_THR_HOVER is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_hover_thrust_set"), events::Log::Warning, + "Hover thrust has been constrained by min/max thrust", _param_mpc_thr_hover.get()); + } + + if (!_param_mpc_use_hte.get() || !_hover_thrust_initialized) { + _control.setHoverThrust(_param_mpc_thr_hover.get()); + _hover_thrust_initialized = true; + } + + // initialize vectors from params and enforce constraints + _param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get())); + _param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get())); + + _takeoff.setSpoolupTime(_param_mpc_spoolup_time.get()); + _takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get()); + _takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get()); + } + + return OK; +} + +PositionControlStates MulticopterLqrControl::set_vehicle_states(const vehicle_local_position_s &local_pos, const vehicle_attitude_s &vehicle_attitude, const vehicle_angular_velocity_s &vehicle_angular_velocity) +{ + PositionControlStates states; + + // only set position states if valid and finite + if (PX4_ISFINITE(local_pos.x) && PX4_ISFINITE(local_pos.y) && local_pos.xy_valid) { + states.position(0) = local_pos.x; + states.position(1) = local_pos.y; + + } else { + states.position(0) = NAN; + states.position(1) = NAN; + } + + if (PX4_ISFINITE(local_pos.z) && local_pos.z_valid) { + states.position(2) = local_pos.z; + + } else { + states.position(2) = NAN; + } + + if (PX4_ISFINITE(local_pos.vx) && PX4_ISFINITE(local_pos.vy) && local_pos.v_xy_valid) { + states.velocity(0) = local_pos.vx; + states.velocity(1) = local_pos.vy; + // states.acceleration(0) = _vel_x_deriv.update(local_pos.vx); + // states.acceleration(1) = _vel_y_deriv.update(local_pos.vy); + + } else { + states.velocity(0) = NAN; + states.velocity(1) = NAN; + // states.acceleration(0) = NAN; + // states.acceleration(1) = NAN; + + // reset derivatives to prevent acceleration spikes when regaining velocity + _vel_x_deriv.reset(); + _vel_y_deriv.reset(); + } + + if (PX4_ISFINITE(local_pos.vz) && local_pos.v_z_valid) { + states.velocity(2) = local_pos.vz; + // states.acceleration(2) = _vel_z_deriv.update(states.velocity(2)); + + } else { + states.velocity(2) = NAN; + // states.acceleration(2) = NAN; + + // reset derivative to prevent acceleration spikes when regaining velocity + _vel_z_deriv.reset(); + } + + Eulerf euler = Eulerf(Quatf(vehicle_attitude.q)); + states.attitude(0) = euler.phi(); + states.attitude(1) = euler.theta(); + states.attitude(2) = euler.psi(); + + states.angular_velocity(0) = vehicle_angular_velocity.xyz[0]; + states.angular_velocity(1) = vehicle_angular_velocity.xyz[1]; + states.angular_velocity(2) = vehicle_angular_velocity.xyz[2]; + + // states.yaw = local_pos.heading; + + return states; +} + +void MulticopterLqrControl::Run() +{ + if (should_exit()) { + // _local_pos_sub.unregisterCallback(); + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + // reschedule backup + // ScheduleDelayed(100_ms); + + parameters_update(false); + + perf_begin(_cycle_perf); + vehicle_local_position_s local_pos; + vehicle_attitude_s vehicle_attitude; + vehicle_angular_velocity_s vehicle_angular_velocity; + + if (_local_pos_sub.update(&local_pos)) { + _local_pos = local_pos; + } else { + local_pos = _local_pos; + } + + if (_vehicle_attitude_sub.update(&vehicle_attitude)) { + _vehicle_attitude = vehicle_attitude; + } else { + vehicle_attitude = _vehicle_attitude; + } + + // if (_local_pos_sub.update(&local_pos) && _vehicle_attitude_sub.update(&vehicle_attitude) && _vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) { + if (_vehicle_angular_velocity_sub.update(&vehicle_angular_velocity)) { + const hrt_abstime time_stamp_now = local_pos.timestamp_sample; + const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); + _time_stamp_last_loop = time_stamp_now; + + // set _dt in controllib Block for BlockDerivative + setDt(dt); + + const bool was_in_failsafe = _in_failsafe; + _in_failsafe = false; + + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); + + if (_param_mpc_use_hte.get()) { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + if (hte.valid) { + _control.updateHoverThrust(hte.hover_thrust); + } + } + } + + PositionControlStates states{set_vehicle_states(local_pos, vehicle_attitude, vehicle_angular_velocity)}; + + if (_vehicle_control_mode.flag_multicopter_position_control_enabled) { + + const bool is_trajectory_setpoint_updated = _trajectory_setpoint_sub.update(&_setpoint); + + // adjust existing (or older) setpoint with any EKF reset deltas + if (_setpoint.timestamp < local_pos.timestamp) { + if (local_pos.vxy_reset_counter != _vxy_reset_counter) { + _setpoint.vx += local_pos.delta_vxy[0]; + _setpoint.vy += local_pos.delta_vxy[1]; + } + + if (local_pos.vz_reset_counter != _vz_reset_counter) { + _setpoint.vz += local_pos.delta_vz; + } + + if (local_pos.xy_reset_counter != _xy_reset_counter) { + _setpoint.x += local_pos.delta_xy[0]; + _setpoint.y += local_pos.delta_xy[1]; + } + + if (local_pos.z_reset_counter != _z_reset_counter) { + _setpoint.z += local_pos.delta_z; + } + + if (local_pos.heading_reset_counter != _heading_reset_counter) { + _setpoint.yaw += local_pos.delta_heading; + } + } + + // update vehicle constraints and handle smooth takeoff + _vehicle_constraints_sub.update(&_vehicle_constraints); + + // fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN + // TODO: this should get obsolete once the takeoff limiting moves into the flight tasks + if (!PX4_ISFINITE(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())) { + _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get(); + } + + if (_vehicle_control_mode.flag_control_offboard_enabled) { + + bool want_takeoff = _vehicle_control_mode.flag_armed && _vehicle_land_detected.landed + && hrt_elapsed_time(&_setpoint.timestamp) < 1_s; + + if (want_takeoff && PX4_ISFINITE(_setpoint.z) + && (_setpoint.z < states.position(2))) { + + _vehicle_constraints.want_takeoff = true; + + } else if (want_takeoff && PX4_ISFINITE(_setpoint.vz) + && (_setpoint.vz < 0.f)) { + + _vehicle_constraints.want_takeoff = true; + + } else if (want_takeoff && PX4_ISFINITE(_setpoint.acceleration[2]) + && (_setpoint.acceleration[2] < 0.f)) { + + _vehicle_constraints.want_takeoff = true; + + } else { + _vehicle_constraints.want_takeoff = false; + } + + // override with defaults + _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get(); + _vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get(); + } + + // handle smooth takeoff + _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, + _vehicle_constraints.want_takeoff, + _vehicle_constraints.speed_up, false, time_stamp_now); + + const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight); + + if (is_trajectory_setpoint_updated) { + // make sure takeoff ramp is not amended by acceleration feed-forward + if (!flying) { + _setpoint.acceleration[2] = NAN; + // hover_thrust maybe reset on takeoff + _control.setHoverThrust(_param_mpc_thr_hover.get()); + } + + const bool not_taken_off = (_takeoff.getTakeoffState() < TakeoffState::rampup); + const bool flying_but_ground_contact = (flying && _vehicle_land_detected.ground_contact); + + if (not_taken_off || flying_but_ground_contact) { + // we are not flying yet and need to avoid any corrections + reset_setpoint_to_nan(_setpoint); + Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust + + // prevent any integrator windup + // _control.resetIntegral(); + } + } + + // // limit tilt during takeoff ramupup + // const float tilt_limit_deg = (_takeoff.getTakeoffState() < TakeoffState::flight) + // ? _param_mpc_tiltmax_lnd.get() : _param_mpc_tiltmax_air.get(); + // _control.setTiltLimit(_tilt_limit_slew_rate.update(math::radians(tilt_limit_deg), dt)); + + // const float speed_up = _takeoff.updateRamp(dt, + // PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get()); + // const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down : + // _param_mpc_z_vel_max_dn.get(); + + // Allow ramping from zero thrust on takeoff + const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f; + + _control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get()); + + // _control.setVelocityLimits( + // _param_mpc_xy_vel_max.get(), + // math::min(speed_up, _param_mpc_z_vel_max_up.get()), // takeoff ramp starts with negative velocity limit + // math::max(speed_down, 0.f)); + + _control.setInputSetpoint(_setpoint); + + // update states + if (!PX4_ISFINITE(_setpoint.z) + && PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON) + && PX4_ISFINITE(local_pos.z_deriv) && local_pos.z_valid && local_pos.v_z_valid) { + // A change in velocity is demanded and the altitude is not controlled. + // Set velocity to the derivative of position + // because it has less bias but blend it in across the landing speed range + // < MPC_LAND_SPEED: ramp up using altitude derivative without a step + // >= MPC_LAND_SPEED: use altitude derivative + float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f); + states.velocity(2) = local_pos.z_deriv * weighting + local_pos.vz * (1.f - weighting); + } + + _control.setState(states); + + // Run position control + if (_control.update(dt)) { + _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now); + + } else { + // Failsafe + if ((time_stamp_now - _last_warn) > 2_s) { + PX4_WARN("invalid setpoints"); + _last_warn = time_stamp_now; + } + + vehicle_local_position_setpoint_s failsafe_setpoint{}; + + failsafe(time_stamp_now, failsafe_setpoint, states, !was_in_failsafe); + + // reset constraints + _vehicle_constraints = {0, NAN, NAN, false, {}}; + + _control.setInputSetpoint(failsafe_setpoint); + // _control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get()); + _control.update(dt); + } + + // Publish internal position control setpoints + // on top of the input/feed-forward setpoints these containt the PID corrections + // This message is used by other modules (such as Landdetector) to determine vehicle intention. + vehicle_local_position_setpoint_s local_pos_sp{}; + _control.getLocalPositionSetpoint(local_pos_sp); + local_pos_sp.timestamp = hrt_absolute_time(); + _local_pos_sp_pub.publish(local_pos_sp); + + // // Publish attitude setpoint output + // vehicle_attitude_setpoint_s attitude_setpoint{}; + // _control.getAttitudeSetpoint(attitude_setpoint); + // attitude_setpoint.timestamp = hrt_absolute_time(); + // _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + + // Publish actuator controls + actuator_controls_s actuator_controls{}; + _control.getActuatorControls(actuator_controls); + actuator_controls.timestamp = hrt_absolute_time(); + _actuator_pub.publish(actuator_controls); + + } else { + // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes + _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true, + time_stamp_now); + } + + // Publish takeoff status + const uint8_t takeoff_state = static_cast(_takeoff.getTakeoffState()); + + if (takeoff_state != _takeoff_status_pub.get().takeoff_state + || !isEqualF(_tilt_limit_slew_rate.getState(), _takeoff_status_pub.get().tilt_limit)) { + _takeoff_status_pub.get().takeoff_state = takeoff_state; + _takeoff_status_pub.get().tilt_limit = _tilt_limit_slew_rate.getState(); + _takeoff_status_pub.get().timestamp = hrt_absolute_time(); + _takeoff_status_pub.update(); + } + + // save latest reset counters + _vxy_reset_counter = local_pos.vxy_reset_counter; + _vz_reset_counter = local_pos.vz_reset_counter; + _xy_reset_counter = local_pos.xy_reset_counter; + _z_reset_counter = local_pos.z_reset_counter; + _heading_reset_counter = local_pos.heading_reset_counter; + } + + perf_end(_cycle_perf); +} + +void MulticopterLqrControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, + const PositionControlStates &states, bool warn) +{ + // do not warn while we are disarmed, as we might not have valid setpoints yet + if (!_vehicle_control_mode.flag_armed) { + warn = false; + } + + // Only react after a short delay + _failsafe_land_hysteresis.set_state_and_update(true, now); + + if (_failsafe_land_hysteresis.get_state()) { + reset_setpoint_to_nan(setpoint); + + if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) { + // don't move along xy + setpoint.vx = setpoint.vy = 0.f; + + if (warn) { + PX4_WARN("Failsafe: stop and wait"); + } + + } else { + // descend with land speed since we can't stop + setpoint.acceleration[0] = setpoint.acceleration[1] = 0.f; + setpoint.vz = _param_mpc_land_speed.get(); + + if (warn) { + PX4_WARN("Failsafe: blind land"); + } + } + + if (PX4_ISFINITE(states.velocity(2))) { + // don't move along z if we can stop in all dimensions + if (!PX4_ISFINITE(setpoint.vz)) { + setpoint.vz = 0.f; + } + + } else { + // emergency descend with a bit below hover thrust + setpoint.vz = NAN; + setpoint.acceleration[2] = .3f; + + if (warn) { + PX4_WARN("Failsafe: blind descend"); + } + } + + _in_failsafe = true; + } +} + +void MulticopterLqrControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint) +{ + setpoint.x = setpoint.y = setpoint.z = NAN; + setpoint.vx = setpoint.vy = setpoint.vz = NAN; + setpoint.yaw = setpoint.yawspeed = NAN; + setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN; + setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN; +} + +int MulticopterLqrControl::task_spawn(int argc, char *argv[]) +{ + bool vtol = false; + + if (argc > 1) { + if (strcmp(argv[1], "vtol") == 0) { + vtol = true; + } + } + + MulticopterLqrControl *instance = new MulticopterLqrControl(vtol); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int MulticopterLqrControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MulticopterLqrControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The controller has two loops: a P loop for position error and a PID loop for velocity error. +Output of the velocity controller is thrust vector that is split to thrust direction +(i.e. rotation matrix for multicopter orientation) and thrust scalar (i.e. multicopter thrust itself). + +The controller doesn't use Euler angles for its work, they are generated only for more human-friendly control and +logging. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("mc_lqr_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int mc_lqr_control_main(int argc, char *argv[]) +{ + return MulticopterLqrControl::main(argc, argv); +} diff --git a/src/modules/mc_lqr_control/MulticopterLqrControl.hpp b/src/modules/mc_lqr_control/MulticopterLqrControl.hpp new file mode 100644 index 000000000000..6964e4b48285 --- /dev/null +++ b/src/modules/mc_lqr_control/MulticopterLqrControl.hpp @@ -0,0 +1,253 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +/** + * Multicopter position controller. + */ + +#pragma once + +// #include "PositionControl/PositionControl.hpp" +#include "LqrControl/LqrControl.hpp" +#include "Takeoff/Takeoff.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace time_literals; + +class MulticopterLqrControl : public ModuleBase, public control::SuperBlock, + public ModuleParams, public px4::WorkItem //, public px4::ScheduledWorkItem +{ +public: + MulticopterLqrControl(bool vtol = false); + ~MulticopterLqrControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ + + orb_advert_t _mavlink_log_pub{nullptr}; + + uORB::PublicationData _takeoff_status_pub {ORB_ID(takeoff_status)}; + uORB::Publication _vehicle_attitude_setpoint_pub {ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _local_pos_sp_pub {ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + uORB::Publication _actuator_pub {ORB_ID(actuator_controls_0)}; + + // uORB::SubscriptionCallbackWorkItem _local_pos_sub {this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ + // uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub {this, ORB_ID(vehicle_attitude)}; + uORB::Subscription _local_pos_sub {ORB_ID(vehicle_local_position)}; /**< vehicle local position */ + uORB::Subscription _vehicle_attitude_sub {ORB_ID(vehicle_attitude)}; + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub {this, ORB_ID(vehicle_angular_velocity)}; + + uORB::SubscriptionInterval _parameter_update_sub {ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _hover_thrust_estimate_sub {ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _trajectory_setpoint_sub {ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_constraints_sub {ORB_ID(vehicle_constraints)}; + uORB::Subscription _vehicle_control_mode_sub {ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub {ORB_ID(vehicle_land_detected)}; + + hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ + + vehicle_local_position_setpoint_s _setpoint {}; + vehicle_control_mode_s _vehicle_control_mode {}; + + vehicle_local_position_s _local_pos {}; + vehicle_attitude_s _vehicle_attitude {}; + + vehicle_constraints_s _vehicle_constraints { + .timestamp = 0, + .speed_up = NAN, + .speed_down = NAN, + .want_takeoff = false, + }; + + vehicle_land_detected_s _vehicle_land_detected { + .timestamp = 0, + .freefall = false, + .ground_contact = true, + .maybe_landed = true, + .landed = true, + }; + + DEFINE_PARAMETERS( + // // Position Control + (ParamFloat) _param_mpc_xy_p, + (ParamFloat) _param_mpc_z_p, + (ParamFloat) _param_mpc_xy_vel_p_acc, + (ParamFloat) _param_mpc_xy_vel_i_acc, + (ParamFloat) _param_mpc_xy_vel_d_acc, + (ParamFloat) _param_mpc_z_vel_p_acc, + (ParamFloat) _param_mpc_z_vel_i_acc, + (ParamFloat) _param_mpc_z_vel_d_acc, + (ParamFloat) _param_mpc_xy_vel_max, + (ParamFloat) _param_mpc_z_v_auto_up, + (ParamFloat) _param_mpc_z_vel_max_up, + (ParamFloat) _param_mpc_z_v_auto_dn, + (ParamFloat) _param_mpc_z_vel_max_dn, + (ParamFloat) _param_mpc_tiltmax_air, + (ParamFloat) _param_mpc_thr_hover, + (ParamBool) _param_mpc_use_hte, + + // Takeoff / Land + (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ + (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ + (ParamFloat) _param_mpc_tko_speed, + (ParamFloat) _param_mpc_land_speed, + + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_xy_cruise, + (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ + (ParamInt) _param_mpc_pos_mode, + (ParamInt) _param_mpc_alt_mode, + (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ + (ParamFloat) _param_mpc_thr_min, + (ParamFloat) _param_mpc_thr_max, + (ParamFloat) _param_mpc_thr_xy_marg, + + (ParamFloat) _param_sys_vehicle_resp, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_acc_down_max, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_acc_hor_max, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_man_y_max, + (ParamFloat) _param_mpc_man_y_tau, + + (ParamFloat) _param_mpc_xy_vel_all, + (ParamFloat) _param_mpc_z_vel_all + ); + + control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ + control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ + control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ + + // PositionControl _control; /**< class for core PID position control */ + LqrControl _control; + + hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */ + + bool _in_failsafe{false}; /**< true if failsafe was entered within current cycle */ + + bool _hover_thrust_initialized{false}; + + /** Timeout in us for trajectory data to get considered invalid */ + static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; + + /** If Flighttask fails, keep 0.2 seconds the current setpoint before going into failsafe land */ + static constexpr uint64_t LOITER_TIME_BEFORE_DESCEND = 200_ms; + + /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off and tilt is limited */ + static constexpr float ALTITUDE_THRESHOLD = 0.3f; + + static constexpr float MAX_SAFE_TILT_DEG = 89.f; // Numerical issues above this value due to tanf + + systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ + SlewRate _tilt_limit_slew_rate; + + uint8_t _vxy_reset_counter{0}; + uint8_t _vz_reset_counter{0}; + uint8_t _xy_reset_counter{0}; + uint8_t _z_reset_counter{0}; + uint8_t _heading_reset_counter{0}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + + /** + * Update our local parameter cache. + * Parameter update can be forced when argument is true. + * @param force forces parameter update. + */ + int parameters_update(bool force); + + /** + * Check for validity of positon/velocity states. + */ + // PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos); + PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos, const vehicle_attitude_s &vehicle_attitude, const vehicle_angular_velocity_s &vehicle_angular_velocity); + + /** + * Failsafe. + * If flighttask fails for whatever reason, then do failsafe. This could + * occur if the commander fails to switch to a mode in case of invalid states or + * setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set + * to true, the failsafe will be initiated immediately. + */ + void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, + bool warn); + + /** + * Reset setpoints to NAN + */ + void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint); +}; diff --git a/src/modules/mc_lqr_control/Takeoff/CMakeLists.txt b/src/modules/mc_lqr_control/Takeoff/CMakeLists.txt new file mode 100644 index 000000000000..a4c129aed661 --- /dev/null +++ b/src/modules/mc_lqr_control/Takeoff/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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(Takeoff + Takeoff.cpp + Takeoff.hpp +) +target_include_directories(Takeoff PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(Takeoff PUBLIC hysteresis) + +px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) diff --git a/src/modules/mc_lqr_control/Takeoff/Takeoff.cpp b/src/modules/mc_lqr_control/Takeoff/Takeoff.cpp new file mode 100644 index 000000000000..1a33377b4017 --- /dev/null +++ b/src/modules/mc_lqr_control/Takeoff/Takeoff.cpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * 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 Takeoff.cpp + */ + +#include "Takeoff.hpp" +#include +#include + +void Takeoff::generateInitialRampValue(float velocity_p_gain) +{ + velocity_p_gain = math::max(velocity_p_gain, 0.01f); + _takeoff_ramp_vz_init = -CONSTANTS_ONE_G / velocity_p_gain; +} + +void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, + const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us) +{ + _spoolup_time_hysteresis.set_state_and_update(armed, now_us); + + switch (_takeoff_state) { + case TakeoffState::disarmed: + if (armed) { + _takeoff_state = TakeoffState::spoolup; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::spoolup: + if (_spoolup_time_hysteresis.get_state()) { + _takeoff_state = TakeoffState::ready_for_takeoff; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::ready_for_takeoff: + if (want_takeoff) { + _takeoff_state = TakeoffState::rampup; + _takeoff_ramp_progress = 0.f; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::rampup: + if (_takeoff_ramp_progress >= 1.f) { + _takeoff_state = TakeoffState::flight; + + } else { + break; + } + + // FALLTHROUGH + case TakeoffState::flight: + if (landed) { + _takeoff_state = TakeoffState::ready_for_takeoff; + } + + break; + + default: + break; + } + + if (armed && skip_takeoff) { + _takeoff_state = TakeoffState::flight; + } + + // TODO: need to consider free fall here + if (!armed) { + _takeoff_state = TakeoffState::disarmed; + } +} + +float Takeoff::updateRamp(const float dt, const float takeoff_desired_vz) +{ + float upwards_velocity_limit = takeoff_desired_vz; + + if (_takeoff_state < TakeoffState::rampup) { + upwards_velocity_limit = _takeoff_ramp_vz_init; + } + + if (_takeoff_state == TakeoffState::rampup) { + if (_takeoff_ramp_time > dt) { + _takeoff_ramp_progress += dt / _takeoff_ramp_time; + + } else { + _takeoff_ramp_progress = 1.f; + } + + if (_takeoff_ramp_progress < 1.f) { + upwards_velocity_limit = _takeoff_ramp_vz_init + _takeoff_ramp_progress * (takeoff_desired_vz - _takeoff_ramp_vz_init); + } + } + + return upwards_velocity_limit; +} diff --git a/src/modules/mc_lqr_control/Takeoff/Takeoff.hpp b/src/modules/mc_lqr_control/Takeoff/Takeoff.hpp new file mode 100644 index 000000000000..ab2a39ffc9d3 --- /dev/null +++ b/src/modules/mc_lqr_control/Takeoff/Takeoff.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 Takeoff.hpp + * + * A class handling all takeoff states and a smooth ramp up of the motors. + */ + +#pragma once + +#include +#include +#include + +using namespace time_literals; + +enum class TakeoffState { + disarmed = takeoff_status_s::TAKEOFF_STATE_DISARMED, + spoolup = takeoff_status_s::TAKEOFF_STATE_SPOOLUP, + ready_for_takeoff = takeoff_status_s::TAKEOFF_STATE_READY_FOR_TAKEOFF, + rampup = takeoff_status_s::TAKEOFF_STATE_RAMPUP, + flight = takeoff_status_s::TAKEOFF_STATE_FLIGHT +}; + +class Takeoff +{ +public: + Takeoff() = default; + ~Takeoff() = default; + + // initialize parameters + void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); } + void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } + + /** + * Calculate a vertical velocity to initialize the takeoff ramp + * that when passed to the velocity controller results in a zero throttle setpoint. + * @param hover_thrust normalized thrsut value with which the vehicle hovers + * @param velocity_p_gain proportional gain of the velocity controller to calculate the thrust + */ + void generateInitialRampValue(const float velocity_p_gain); + + /** + * Update the state for the takeoff. + * @param setpoint a vehicle_local_position_setpoint_s structure + * @return true if setpoint has updated correctly + */ + void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, + const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us); + + /** + * Update and return the velocity constraint ramp value during takeoff. + * By ramping up _takeoff_ramp_vz during the takeoff and using it to constain the maximum climb rate a smooth takeoff behavior is achieved. + * Returns zero on the ground and takeoff_desired_vz in flight. + * @param dt time in seconds since the last call/loop iteration + * @param takeoff_desired_vz end value for the velocity ramp + * @return true if setpoint has updated correctly + */ + float updateRamp(const float dt, const float takeoff_desired_vz); + + TakeoffState getTakeoffState() { return _takeoff_state; } + +private: + TakeoffState _takeoff_state = TakeoffState::disarmed; + + systemlib::Hysteresis _spoolup_time_hysteresis{false}; ///< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed + + float _takeoff_ramp_time{0.f}; + float _takeoff_ramp_vz_init{0.f}; ///< verticval velocity resulting in zero thrust + float _takeoff_ramp_progress{0.f}; ///< asecnding from 0 to 1 +}; diff --git a/src/modules/mc_lqr_control/Takeoff/TakeoffTest.cpp b/src/modules/mc_lqr_control/Takeoff/TakeoffTest.cpp new file mode 100644 index 000000000000..865514dc9875 --- /dev/null +++ b/src/modules/mc_lqr_control/Takeoff/TakeoffTest.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 +#include +#include + +TEST(TakeoffTest, Initialization) +{ + Takeoff takeoff; + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); +} + +TEST(TakeoffTest, RegularTakeoffRamp) +{ + Takeoff takeoff; + takeoff.setSpoolupTime(1.f); + takeoff.setTakeoffRampTime(2.0); + takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f); + + // disarmed, landed, don't want takeoff + takeoff.updateTakeoffState(false, true, false, 1.f, false, 0); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::disarmed); + + // armed, not landed anymore, don't want takeoff + takeoff.updateTakeoffState(true, false, false, 1.f, false, 500_ms); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::spoolup); + + // armed, not landed, don't want takeoff yet, spoolup time passed + takeoff.updateTakeoffState(true, false, false, 1.f, false, 2_s); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::ready_for_takeoff); + + // armed, not landed, want takeoff + takeoff.updateTakeoffState(true, false, true, 1.f, false, 3_s); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::rampup); + + // armed, not landed, want takeoff, ramping up + takeoff.updateTakeoffState(true, false, true, 1.f, false, 4_s); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 0.f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), .5f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f); + EXPECT_FLOAT_EQ(takeoff.updateRamp(.5f, 1.5f), 1.5f); + + // armed, not landed, want takeoff, rampup time passed + takeoff.updateTakeoffState(true, false, true, 1.f, false, 6500_ms); + EXPECT_EQ(takeoff.getTakeoffState(), TakeoffState::flight); +} diff --git a/src/modules/mc_lqr_control/mc_lqr_control_params.c b/src/modules/mc_lqr_control/mc_lqr_control_params.c new file mode 100644 index 000000000000..0dd58dfbbb7a --- /dev/null +++ b/src/modules/mc_lqr_control/mc_lqr_control_params.c @@ -0,0 +1,836 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2016 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 mc_lqr_control_params.c + * Multicopter position controller parameters. + * + * @author Anton Babushkin + */ + +/** + * Minimum collective thrust in auto thrust control + * + * It's recommended to set it > 0 to avoid free fall with zero thrust. + * Note: Without airmode zero thrust leads to zero roll/pitch control authority. (see MC_AIRMODE) + * + * @unit norm + * @min 0.05 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); + +/** + * Hover thrust + * + * Vertical thrust required to hover. + * This value is mapped to center stick for manual throttle control. + * With this value set to the thrust required to hover, transition + * from manual to Altitude or Position mode while hovering will occur with the + * throttle stick near center, which is then interpreted as (near) + * zero demand for vertical speed. + * + * This parameter is also important for the landing detection to work correctly. + * + * @unit norm + * @min 0.1 + * @max 0.8 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); + +/** + * Hover thrust source selector + * + * Set false to use the fixed parameter MPC_THR_HOVER + * Set true to use the value computed by the hover thrust estimator + * + * @boolean + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_USE_HTE, 1); + +/** + * Thrust curve in Manual Mode + * + * This parameter defines how the throttle stick input is mapped to commanded thrust + * in Manual/Stabilized flight mode. + * + * In case the default is used ('Rescale to hover thrust'), the stick input is linearly + * rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). + * + * Select 'No Rescale' to directly map the stick 1:1 to the output. This can be useful + * in case the hover thrust is very low and the default would lead to too much distortion + * (e.g. if hover thrust is set to 20%, 80% of the upper thrust range is squeezed into the + * upper half of the stick range). + * + * Note: In case MPC_THR_HOVER is set to 50%, the modes 0 and 1 are the same. + * + * @value 0 Rescale to hover thrust + * @value 1 No Rescale + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_THR_CURVE, 0); + +/** + * Horizontal thrust margin + * + * Margin that is kept for horizontal control when prioritizing vertical thrust. + * To avoid completely starving horizontal control with high vertical error. + * + * @unit norm + * @min 0.0 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_XY_MARG, 0.3f); + +/** + * Maximum thrust in auto thrust control + * + * Limit max allowed thrust + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); + +/** + * Minimum manual thrust + * + * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + * With MC_AIRMODE set to 1, this can safely be set to 0. + * + * @unit norm + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f); + +/** + * Proportional gain for vertical position error + * + * @min 0.0 + * @max 1.5 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); + +/** + * Proportional gain for vertical velocity error + * + * defined as correction acceleration in m/s^2 per m/s velocity error + * + * @min 2.0 + * @max 15.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P_ACC, 4.0f); + +/** + * Integral gain for vertical velocity error + * + * defined as correction acceleration in m/s^2 per m velocity integral + * + * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + * + * @min 0.2 + * @max 3.0 + * @decimal 3 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.0f); + +/** + * Differential gain for vertical velocity error + * + * defined as correction acceleration in m/s^2 per m/s^2 velocity derivative + * + * @min 0.0 + * @max 2.0 + * @decimal 3 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.0f); + +/** + * Automatic ascent velocity + * + * Ascent velocity in auto modes. + * For manual modes and offboard, see MPC_Z_VEL_MAX_UP + * + * @unit m/s + * @min 0.5 + * @max 8.0 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_UP, 3.f); + +/** + * Maximum ascent velocity + * + * Ascent velocity in manual modes and offboard. + * For auto modes, see MPC_Z_V_AUTO_UP + * + * @unit m/s + * @min 0.5 + * @max 8.0 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.f); + +/** + * Automatic descent velocity + * + * Descent velocity in auto modes. + * For manual modes and offboard, see MPC_Z_VEL_MAX_DN + * + * @unit m/s + * @min 0.5 + * @max 4.0 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.f); + +/** + * Maximum descent velocity + * + * Descent velocity in manual modes and offboard. + * For auto modes, see MPC_Z_V_AUTO_DN + * + * @unit m/s + * @min 0.5 + * @max 4.0 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.f); + +/** + * Proportional gain for horizontal position error + * + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f); + +/** + * Proportional gain for horizontal velocity error + * + * defined as correction acceleration in m/s^2 per m/s velocity error + * + * @min 1.2 + * @max 5.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P_ACC, 1.8f); + +/** + * Integral gain for horizontal velocity error + * + * defined as correction acceleration in m/s^2 per m velocity integral + * Non-zero value allows to eliminate steady state errors in the presence of disturbances like wind. + * + * @min 0.0 + * @max 60.0 + * @decimal 3 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I_ACC, 0.4f); + +/** + * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * defined as correction acceleration in m/s^2 per m/s^2 velocity derivative + * + * @min 0.1 + * @max 2.0 + * @decimal 3 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D_ACC, 0.2f); + +/** + * Default horizontal velocity in mission + * + * Horizontal velocity used when flying autonomously in e.g. Missions, RTL, Goto. + * + * @unit m/s + * @min 3.0 + * @max 20.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f); + +/** + * Proportional gain for horizontal trajectory position error + * + * @min 0.1 + * @max 1.0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f); + +/** + * Maximum horizontal error allowed by the trajectory generator + * + * The integration speed of the trajectory setpoint is linearly + * reduced with the horizontal position tracking error. When the + * error is above this parameter, the integration of the + * trajectory is stopped to wait for the drone. + * + * This value can be adjusted depending on the tracking + * capabilities of the vehicle. + * + * @min 0.1 + * @max 10.0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f); + +/** + * Maximum horizontal velocity setpoint for manual controlled mode + * + * If velocity setpoint larger than MPC_XY_VEL_MAX is set, then + * the setpoint will be capped to MPC_XY_VEL_MAX + * + * @unit m/s + * @min 3.0 + * @max 20.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.0f); + +/** + * Maximum horizontal velocity + * + * Maximum horizontal velocity in AUTO mode. If higher speeds + * are commanded in a mission they will be capped to this velocity. + * + * @unit m/s + * @min 0.0 + * @max 20.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.0f); + +/** + * Maximum tilt angle in air + * + * Limits maximum tilt in AUTO and POSCTRL modes during flight. + * + * @unit deg + * @min 20.0 + * @max 89.0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); + +/** + * Maximum tilt during landing + * + * Limits maximum tilt angle on landing. + * + * @unit deg + * @min 10.0 + * @max 89.0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); + +/** + * Landing descend rate + * + * @unit m/s + * @min 0.6 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); + +/** + * Enable user assisted descent speed for autonomous land routine. + * + * When enabled, descent speed will be: + * stick full up - 0 + * stick centered - MPC_LAND_SPEED + * stick full down - 2 * MPC_LAND_SPEED + * + * @min 0 + * @max 1 + * @value 0 Fixed descent speed of MPC_LAND_SPEED + * @value 1 User assisted descent speed + */ +PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0); + +/** + * Takeoff climb rate + * + * @unit m/s + * @min 1 + * @max 5 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_SPEED, 1.5f); + +/** + * Maximal tilt angle in manual or altitude mode + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_TILT_MAX, 35.0f); + +/** + * Max manual yaw rate + * + * @unit deg/s + * @min 0.0 + * @max 400 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 150.0f); + +/** + * Manual yaw rate input filter time constant + * + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0.0 + * @max 5.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_Y_TAU, 0.08f); + +/** + * Deadzone of sticks where position hold is enabled + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_DZ, 0.1f); + +/** + * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @max 3.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.8f); + +/** + * Maximum vertical velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @max 3.0 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.6f); + +/** + * Low pass filter cut freq. for numerical velocity derivative + * + * @unit Hz + * @min 0.0 + * @max 10 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); + +/** + * Maximum horizontal acceleration for auto mode and for manual mode + * + * MPC_POS_MODE + * 1 just deceleration + * 3 acceleration and deceleration + * 4 just acceleration + * + * @unit m/s^2 + * @min 2.0 + * @max 15.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +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 4. + * + * @unit m/s^2 + * @min 2.0 + * @max 15.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ + +PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 3.0f); + +/** + * Maximum vertical acceleration in velocity controlled modes upward + * + * @unit m/s^2 + * @min 2.0 + * @max 15.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 4.0f); + +/** + * Maximum vertical acceleration in velocity controlled modes down + * + * @unit m/s^2 + * @min 2.0 + * @max 15.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.0f); + +/** + * Maximum jerk limit + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother vehicle motions, but it also limits its + * agility (how fast it can change directions or break). + * + * 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 3 or 4. + * + * @unit m/s^3 + * @min 0.5 + * @max 500.0 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_JERK_MAX, 8.0f); + +/** + * Jerk limit in auto mode + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother vehicle motions, but it also limits its + * agility. + * + * @unit m/s^3 + * @min 1.0 + * @max 80.0 + * @increment 1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_JERK_AUTO, 4.0f); + +/** + * Altitude control mode. + * + * Set to 0 to control height relative to the earth frame origin. This origin may move up and down in + * flight due to sensor drift. + * Set to 1 to control height relative to estimated distance to ground. The vehicle will move up and down + * with terrain height variation. Requires a distance to ground sensor. The height controller will + * revert to using height above origin if the distance to ground estimate becomes invalid as indicated + * by the local_position.distance_bottom_valid message being false. + * Set to 2 to control height relative to ground (requires a distance sensor) when stationary and relative + * to earth frame origin when moving horizontally. + * The speed threshold is controlled by the MPC_HOLD_MAX_XY parameter. + * + * @min 0 + * @max 2 + * @value 0 Altitude following + * @value 1 Terrain following + * @value 2 Terrain hold + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_ALT_MODE, 0); + +/** + * Manual position control stick exponential curve sensitivity + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve (default) + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.6f); + +/** + * Manual control stick vertical exponential curve + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve (default) + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_MAN_EXPO, 0.6f); + +/** + * Manual control stick yaw rotation exponential curve + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve (default) + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_YAW_EXPO, 0.6f); + +/** + * Max yaw rate in auto mode + * + * Limit the rate of change of the yaw setpoint in autonomous mode + * to avoid large control output and mixer saturation. + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.0f); + +/** + * Altitude for 1. step of slow landing (descend) + * + * Below this altitude descending velocity gets limited to a value + * between "MPC_Z_VEL_MAX_DN" (or "MPC_Z_V_AUTO_DN") and "MPC_LAND_SPEED" + * Value needs to be higher than "MPC_LAND_ALT2" + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f); + +/** + * Altitude for 2. step of slow landing (landing) + * + * Below this altitude descending velocity gets + * limited to "MPC_LAND_SPEED". + * Value needs to be lower than "MPC_LAND_ALT1" + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f); + +/** + * Position control smooth takeoff ramp time constant + * + * Increasing this value will make automatic and manual takeoff slower. + * If it's too slow the drone might scratch the ground and tip over. + * A time constant of 0 disables the ramp + * + * @min 0 + * @max 5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f); + +/** + * Manual-Position control sub-mode + * + * The supported sub-modes are: + * 0 Simple position control where sticks map directly to velocity setpoints + * without smoothing. Useful for velocity control tuning. + * 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 3 Smooth position control (Jerk optimized) + * @value 4 Acceleration based input + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_POS_MODE, 4); + +/** + * Enforced delay between arming and takeoff + * + * For altitude controlled modes the time from arming the motors until + * a takeoff is possible gets forced to be at least MPC_SPOOLUP_TIME seconds + * to ensure the motors and propellers can sppol up and reach idle speed before + * getting commanded to spin faster. This delay is particularly useful for vehicles + * with slow motor spin-up e.g. because of large propellers. + * + * @min 0 + * @max 10 + * @unit s + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f); + +/** + * Yaw mode. + * + * Specifies the heading in Auto. + * + * @min 0 + * @max 4 + * @value 0 towards waypoint + * @value 1 towards home + * @value 2 away from home + * @value 3 along trajectory + * @value 4 towards waypoint (yaw first) + * @group Mission + */ +PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); + +/** + * Responsiveness + * + * Changes the overall responsiveness of the vehicle. + * The higher the value, the faster the vehicle will react. + * + * If set to a value greater than zero, other parameters are automatically set (such as + * the acceleration or jerk limits). + * If set to a negative value, the existing individual parameters are used. + * + * @min -1 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(SYS_VEHICLE_RESP, -0.4f); + +/** + * Overall Horizonal Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_XY_VEL_MAX or MPC_VEL_MANUAL). + * If set to a negative value, the existing individual parameters are used. + * + * @min -20 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_ALL, -10.0f); + +/** + * Overall Vertical Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_Z_VEL_MAX_UP or MPC_LAND_SPEED). + * If set to a negative value, the existing individual parameters are used. + * + * @min -3 + * @max 8 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_ALL, -3.0f);