diff --git a/src/modules/spacecraft/CMakeLists.txt b/src/modules/spacecraft/CMakeLists.txt index a4a3c6ded4..31ef51ff20 100644 --- a/src/modules/spacecraft/CMakeLists.txt +++ b/src/modules/spacecraft/CMakeLists.txt @@ -34,6 +34,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(SpacecraftRateControl) add_subdirectory(SpacecraftAttitudeControl) +add_subdirectory(SpacecraftPositionControl) px4_add_module( MODULE modules__spacecraft @@ -52,4 +53,5 @@ px4_add_module( px4_work_queue SpacecraftRateControl SpacecraftAttitudeControl + SpacecraftPositionControl ) diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp b/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp index 5358fc7fcf..1e87b94295 100644 --- a/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp @@ -136,4 +136,3 @@ private: (ParamFloat) _param_sc_man_y_scale /**< scaling factor from stick to yaw rate */ ) }; - diff --git a/src/modules/spacecraft/SpacecraftHandler.cpp b/src/modules/spacecraft/SpacecraftHandler.cpp index 175ad8f8ca..931e1069e1 100644 --- a/src/modules/spacecraft/SpacecraftHandler.cpp +++ b/src/modules/spacecraft/SpacecraftHandler.cpp @@ -72,6 +72,7 @@ void SpacecraftHandler::Run() _timestamp = hrt_absolute_time(); _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + _spacecraft_position_control.updatePositionControl(); _spacecraft_attitude_control.updateAttitudeControl(); _spacecraft_rate_control.updateRateControl(); diff --git a/src/modules/spacecraft/SpacecraftHandler.hpp b/src/modules/spacecraft/SpacecraftHandler.hpp index c9b9e293e8..732347cd97 100644 --- a/src/modules/spacecraft/SpacecraftHandler.hpp +++ b/src/modules/spacecraft/SpacecraftHandler.hpp @@ -65,6 +65,7 @@ // Local includes #include "SpacecraftRateControl/SpacecraftRateControl.hpp" #include "SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp" +#include "SpacecraftPositionControl/SpacecraftPositionControl.hpp" class SpacecraftHandler : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { @@ -109,6 +110,7 @@ private: // Class instances SpacecraftRateControl _spacecraft_rate_control{this}; SpacecraftAttitudeControl _spacecraft_attitude_control{this}; + SpacecraftPositionControl _spacecraft_position_control{this}; // Variables hrt_abstime _timestamp{0}; diff --git a/src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt new file mode 100644 index 0000000000..6a3bc3f018 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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(PositionControl) + +px4_add_library(SpacecraftPositionControl + SpacecraftPositionControl.cpp +) + +target_link_libraries(SpacecraftPositionControl PUBLIC mathlib) +target_link_libraries(SpacecraftPositionControl PUBLIC PositionControlLibrary) +target_include_directories(SpacecraftPositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/CMakeLists.txt new file mode 100644 index 0000000000..58d10293f1 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(PositionControlLibrary + ControlMath.cpp + ControlMath.hpp + PositionControl.cpp + PositionControl.hpp +) +target_include_directories(PositionControlLibrary PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +# TODO: add unit tests +# px4_add_unit_gtest(SRC ScControlMathTest.cpp LINKLIBS SpacecraftPositionControl) +# px4_add_unit_gtest(SRC ScPositionControlTest.cpp LINKLIBS SpacecraftPositionControl) diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp new file mode 100644 index 0000000000..d2228d5597 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.cpp @@ -0,0 +1,260 @@ +/**************************************************************************** + * + * 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 ControlMath.cpp + */ + +#include "ControlMath.hpp" +#include +#include +#include + +using namespace matrix; + +namespace ControlMath +{ +void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +{ + bodyzToAttitude(-thr_sp, yaw_sp, att_sp); + att_sp.thrust_body[2] = -thr_sp.length(); +} + +void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_angle) +{ + // determine tilt + const float dot_product_unit = body_unit.dot(world_unit); + float angle = acosf(dot_product_unit); + // limit tilt + angle = math::min(angle, max_angle); + Vector3f rejection = body_unit - (dot_product_unit * world_unit); + + // corner case exactly parallel vectors + if (rejection.norm_squared() < FLT_EPSILON) { + rejection(0) = 1.f; + } + + body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit(); +} + +void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +{ + // zero vector, no direction, set safe level value + if (body_z.norm_squared() < FLT_EPSILON) { + body_z(2) = 1.f; + } + + body_z.normalize(); + + // vector of desired yaw direction in XY plane, rotated by PI/2 + const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f}; + + // desired body_x axis, orthogonal to body_z + Vector3f body_x = y_C % body_z; + + // keep nose to front while inverted upside down + if (body_z(2) < 0.f) { + body_x = -body_x; + } + + if (fabsf(body_z(2)) < 0.000001f) { + // desired thrust is in XY plane, set X downside to construct correct matrix, + // but yaw component will not be used actually + body_x.zero(); + body_x(2) = 1.f; + } + + body_x.normalize(); + + // desired body_y axis + const Vector3f body_y = body_z % body_x; + + Dcmf R_sp; + + // fill rotation matrix + for (int i = 0; i < 3; i++) { + R_sp(i, 0) = body_x(i); + R_sp(i, 1) = body_y(i); + R_sp(i, 2) = body_z(i); + } + + // copy quaternion setpoint to attitude setpoint topic + const Quatf q_sp{R_sp}; + q_sp.copyTo(att_sp.q_d); + + // calculate euler angles, for logging only, must not be used for control + const Eulerf euler{R_sp}; + att_sp.roll_body = euler.phi(); + att_sp.pitch_body = euler.theta(); + att_sp.yaw_body = euler.psi(); +} + +Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) +{ + if (Vector2f(v0 + v1).norm() <= max) { + // vector does not exceed maximum magnitude + return v0 + v1; + + } else if (v0.length() >= max) { + // the magnitude along v0, which has priority, already exceeds maximum. + return v0.normalized() * max; + + } else if (fabsf(Vector2f(v1 - v0).norm()) < 0.001f) { + // the two vectors are equal + return v0.normalized() * max; + + } else if (v0.length() < 0.001f) { + // the first vector is 0. + return v1.normalized() * max; + + } else { + // vf = final vector with ||vf|| <= max + // s = scaling factor + // u1 = unit of v1 + // vf = v0 + v1 = v0 + s * u1 + // constraint: ||vf|| <= max + // + // solve for s: ||vf|| = ||v0 + s * u1|| <= max + // + // Derivation: + // For simplicity, replace v0 -> v, u1 -> u + // v0(0/1/2) -> v0/1/2 + // u1(0/1/2) -> u0/1/2 + // + // ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v2+s*u2)^2 = max^2 + // v0^2+2*s*u0*v0+s^2*u0^2 + v1^2+2*s*u1*v1+s^2*u1^2 + v2^2+2*s*u2*v2+s^2*u2^2 = max^2 + // s^2*(u0^2+u1^2+u2^2) + s*2*(u0*v0+u1*v1+u2*v2) + (v0^2+v1^2+v2^2-max^2) = 0 + // + // quadratic equation: + // -> s^2*a + s*b + c = 0 with solution: s1/2 = (-b +- sqrt(b^2 - 4*a*c))/(2*a) + // + // b = 2 * u.dot(v) + // a = 1 (because u is normalized) + // c = (v0^2+v1^2+v2^2-max^2) = -max^2 + ||v||^2 + // + // sqrt(b^2 - 4*a*c) = + // sqrt(4*u.dot(v)^2 - 4*(||v||^2 - max^2)) = 2*sqrt(u.dot(v)^2 +- (||v||^2 -max^2)) + // + // s1/2 = ( -2*u.dot(v) +- 2*sqrt(u.dot(v)^2 - (||v||^2 -max^2)) / 2 + // = -u.dot(v) +- sqrt(u.dot(v)^2 - (||v||^2 -max^2)) + // m = u.dot(v) + // s = -m + sqrt(m^2 - c) + // + // + // + // notes: + // - s (=scaling factor) needs to be positive + // - (max - ||v||) always larger than zero, otherwise it never entered this if-statement + Vector2f u1 = v1.normalized(); + float m = u1.dot(v0); + float c = v0.dot(v0) - max * max; + float s = -m + sqrtf(m * m - c); + return v0 + u1 * s; + } +} + +bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r, + const Vector3f &line_a, const Vector3f &line_b, Vector3f &res) +{ + // project center of sphere on line normalized AB + Vector3f ab_norm = line_b - line_a; + + if (ab_norm.length() < 0.01f) { + return true; + } + + ab_norm.normalize(); + Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + if (sphere_r > cd_len) { + // we have triangle CDX with known CD and CX = R, find DX + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + + if ((sphere_c - line_b) * ab_norm > 0.f) { + // target waypoint is already behind us + res = line_b; + + } else { + // target is in front of us + res = d + ab_norm * dx_len; // vector A->B on line + } + + return true; + + } else { + + // have no roots, return D + res = d; // go directly to line + + // previous waypoint is still in front of us + if ((sphere_c - line_a) * ab_norm < 0.f) { + res = line_a; + } + + // target waypoint is already behind us + if ((sphere_c - line_b) * ab_norm > 0.f) { + res = line_b; + } + + return false; + } +} + +void addIfNotNan(float &setpoint, const float addition) +{ + if (PX4_ISFINITE(setpoint) && PX4_ISFINITE(addition)) { + // No NAN, add to the setpoint + setpoint += addition; + + } else if (!PX4_ISFINITE(setpoint)) { + // Setpoint NAN, take addition + setpoint = addition; + } + + // Addition is NAN or both are NAN, nothing to do +} + +void addIfNotNanVector3f(Vector3f &setpoint, const Vector3f &addition) +{ + for (int i = 0; i < 3; i++) { + addIfNotNan(setpoint(i), addition(i)); + } +} + +void setZeroIfNanVector3f(Vector3f &vector) +{ + // Adding zero vector overwrites elements that are NaN with zero + addIfNotNanVector3f(vector, Vector3f()); +} +} \ No newline at end of file diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.hpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.hpp new file mode 100644 index 0000000000..c2d9b8b7f8 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ControlMath.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 ControlMath.hpp + * + * Simple functions for vector manipulation that do not fit into matrix lib. + * These functions are specific for controls. + */ + +#pragma once + +#include +#include + +namespace ControlMath +{ +/** + * Converts thrust vector and yaw set-point to a desired attitude. + * @param thr_sp desired 3D thrust vector + * @param yaw_sp the desired yaw + * @param att_sp attitude setpoint to fill + */ +void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); + +/** + * Limits the tilt angle between two unit vectors + * @param body_unit unit vector that will get adjusted if angle is too big + * @param world_unit fixed vector to measure the angle against + * @param max_angle maximum tilt angle between vectors in radians + */ +void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit, const float max_angle); + +/** + * Converts a body z vector and yaw set-point to a desired attitude. + * @param body_z a world frame 3D vector in direction of the desired body z axis + * @param yaw_sp the desired yaw setpoint + * @param att_sp attitude setpoint to fill + */ +void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); + +/** + * Outputs the sum of two vectors but respecting the limits and priority. + * The sum of two vectors are constraint such that v0 has priority over v1. + * This means that if the length of (v0+v1) exceeds max, then it is constraint such + * that v0 has priority. + * + * @param v0 a 2D vector that has priority given the maximum available magnitude. + * @param v1 a 2D vector that less priority given the maximum available magnitude. + * @return 2D vector + */ +matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f &v1, const float &max); + +/** + * This method was used for smoothing the corners along two lines. + * + * @param sphere_c + * @param sphere_r + * @param line_a + * @param line_b + * @param res + * return boolean + * + * Note: this method is not used anywhere and first requires review before usage. + */ +bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r, const matrix::Vector3f &line_a, + const matrix::Vector3f &line_b, matrix::Vector3f &res); + +/** + * Adds e.g. feed-forward to the setpoint making sure existing or added NANs have no influence on control. + * This function is udeful to support all the different setpoint combinations of position, velocity, acceleration with NAN representing an uncommitted value. + * @param setpoint existing possibly NAN setpoint to add to + * @param addition value/NAN to add to the setpoint + */ +void addIfNotNan(float &setpoint, const float addition); + +/** + * _addIfNotNan for Vector3f treating each element individually + * @see _addIfNotNan + */ +void addIfNotNanVector3f(matrix::Vector3f &setpoint, const matrix::Vector3f &addition); + +/** + * Overwrites elements of a Vector3f which are NaN with zero + * @param vector possibly containing NAN elements + */ +void setZeroIfNanVector3f(matrix::Vector3f &vector); +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp new file mode 100644 index 0000000000..6d9ea12767 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.cpp @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * 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 PositionControl.cpp + */ + +#include "PositionControl.hpp" +#include "ControlMath.hpp" +#include +#include +#include +#include +#include + +using namespace matrix; + +const trajectory_setpoint_s ScPositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; + +void ScPositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) +{ + _gain_vel_p = P; + _gain_vel_i = I; + _gain_vel_d = D; +} + +void ScPositionControl::setPositionGains(const Vector3f &P, const Vector3f &I) +{ + _gain_pos_p = P; + _gain_pos_i = I; +} + +void ScPositionControl::setPositionIntegralLimits(const float lim) +{ + _pos_int_lim = lim; +} + +void ScPositionControl::setVelocityIntegralLimits(const float lim) +{ + _vel_int_lim = lim; +} + +void ScPositionControl::setVelocityLimits(const float vel_limit) +{ + _lim_vel = vel_limit; +} + +void ScPositionControl::setThrustLimit(const float max) +{ + _lim_thr_max = max; +} + +void ScPositionControl::setState(const PositionControlStates &states) +{ + _pos = states.position; + _vel = states.velocity; + _vel_dot = states.acceleration; + _att_q = states.quaternion; +} + +void ScPositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint) +{ + _pos_sp = Vector3f(setpoint.position); + _vel_sp = Vector3f(setpoint.velocity); + _acc_sp = Vector3f(setpoint.acceleration); + _quat_sp = Quatf(setpoint.attitude); +} + +bool ScPositionControl::update(const float dt) +{ + bool valid = _inputValid(); + + if (valid) { + _positionControl(dt); + _velocityControl(dt); + } + + // There has to be a valid output acceleration and thrust setpoint otherwise something went wrong + return valid && _acc_sp.isAllFinite() && _thr_sp.isAllFinite(); +} + +void ScPositionControl::_positionControl(const float dt) +{ + // Constrain vertical velocity integral + _pos_int(0) = math::constrain(_vel_int(0), -_pos_int_lim, _pos_int_lim); + _pos_int(1) = math::constrain(_vel_int(1), -_pos_int_lim, _pos_int_lim); + _pos_int(2) = math::constrain(_vel_int(2), -_pos_int_lim, _pos_int_lim); + + // P-position controller + ControlMath::setZeroIfNanVector3f(_pos_sp); + Vector3f pos_error = _pos_sp - _pos; + Vector3f vel_sp_position = pos_error.emult(_gain_pos_p) + _pos_int; + + // Update integral part of position control + _vel_int += pos_error.emult(_gain_pos_i) * dt; + + // 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 velocity setpoints + _vel_sp(0) = math::constrain(_vel_sp(0), -_lim_vel, _lim_vel); + _vel_sp(1) = math::constrain(_vel_sp(1), -_lim_vel, _lim_vel); + _vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel, _lim_vel); + +} + +void ScPositionControl::_velocityControl(const float dt) +{ + // Constrain vertical velocity integral + // _vel_int(2) = math::constrain(_vel_int(2), -CONSTANTS_ONE_G, CONSTANTS_ONE_G); + // Constrain vertical velocity integral + _vel_int(0) = math::constrain(_vel_int(0), -_vel_int_lim, _vel_int_lim); + _vel_int(1) = math::constrain(_vel_int(1), -_vel_int_lim, _vel_int_lim); + _vel_int(2) = math::constrain(_vel_int(2), -_vel_int_lim, _vel_int_lim); + + + // 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); + + // Accelaration to Thrust + _thr_sp = _acc_sp; + _thr_sp(0) = math::constrain(_thr_sp(0), -_lim_thr_max, _lim_thr_max); + _thr_sp(1) = math::constrain(_thr_sp(1), -_lim_thr_max, _lim_thr_max); + _thr_sp(2) = math::constrain(_thr_sp(2), -_lim_thr_max, _lim_thr_max); + + // 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; +} + +bool ScPositionControl::_inputValid() +{ + bool valid = true; + + // Every axis x, y, z needs to have some setpoint + for (int i = 0; i <= 2; i++) { + valid = valid && (PX4_ISFINITE(_pos_sp(i)) || PX4_ISFINITE(_vel_sp(i)) || PX4_ISFINITE(_acc_sp(i))); + } + + // x and y input setpoints always have to come in pairs + valid = valid && (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1))); + valid = valid && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1))); + valid = valid && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1))); + + // 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)); + } + } + + return valid; +} + +void ScPositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, + vehicle_attitude_s &v_att) const +{ + // Set thrust setpoint + const Dcmf R_to_body(Quatf(v_att.q).inversed()); + matrix::Vector3f b_thr_sp = R_to_body * _thr_sp; + attitude_setpoint.thrust_body[0] = b_thr_sp(0); + attitude_setpoint.thrust_body[1] = b_thr_sp(1); + attitude_setpoint.thrust_body[2] = b_thr_sp(2); + + // Bypass attitude control by giving same attitude setpoint to att control + if (PX4_ISFINITE(_quat_sp(0)) && PX4_ISFINITE(_quat_sp(1)) && PX4_ISFINITE(_quat_sp(2)) && PX4_ISFINITE(_quat_sp(3))) { + attitude_setpoint.q_d[0] = _quat_sp(0); + attitude_setpoint.q_d[1] = _quat_sp(1); + attitude_setpoint.q_d[2] = _quat_sp(2); + attitude_setpoint.q_d[3] = _quat_sp(3); + + } else { + attitude_setpoint.q_d[0] = v_att.q[0]; + attitude_setpoint.q_d[1] = v_att.q[1]; + attitude_setpoint.q_d[2] = v_att.q[2]; + attitude_setpoint.q_d[3] = v_att.q[3]; + } +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp new file mode 100644 index 0000000000..d5a910e41c --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/PositionControl.hpp @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * 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 PositionControl.hpp + * + * A cascaded position controller for position/velocity control only. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +struct PositionControlStates { + matrix::Vector3f position; + matrix::Vector3f velocity; + matrix::Vector3f acceleration; + matrix::Quatf quaternion; // bypassed to attitude controller +}; + +/** + * Core Position-Control for spacecrafts. + * This class contains P-controller for position and + * PID-controller for velocity. + * + * Inputs: + * vehicle position/velocity/attitude + * desired set-point position/velocity/thrust/attitude + * Output + * thrust vector and quaternion for attitude control + * + * A setpoint that is NAN is considered as not set. + * If there is a position/velocity- and thrust-setpoint present, then + * the thrust-setpoint is ommitted and recomputed from position-velocity-PID-loop. + */ +class ScPositionControl +{ +public: + + ScPositionControl() = default; + ~ScPositionControl() = default; + + /** + * Set the position control gains + * @param P 3D vector of proportional gains for x,y,z axis + * @param I 3D vector of integral gains for x,y,z axis + */ + void setPositionGains(const matrix::Vector3f &P, const matrix::Vector3f &I); + + /** + * @brief Set the Position Integral Limits object + * + * @param lim float limit to be set (on all axis) + */ + void setPositionIntegralLimits(const float lim); + + /** + * Set the velocity control gains + * @param P 3D vector of proportional gains for x,y,z axis + * @param I 3D vector of integral gains + * @param D 3D vector of derivative gains + */ + void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); + + /** + * Set the maximum velocity to execute with feed forward and position control + * @param vel_limit velocity limit + */ + void setVelocityLimits(const float vel_limit); + + /** + * @brief Set the Velocity Integral Limits object + * + * @param lim float limit to be set (on all axis) + */ + void setVelocityIntegralLimits(const float lim); + + /** + * 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 setThrustLimit(const float max); + + /** + * 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 setpoints including feed-forwards to execute in update() + */ + void setInputSetpoint(const trajectory_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); + + /** + * Set the integral term in xy to 0. + * @see _vel_int + */ + void resetIntegral() { + _pos_int.setZero(); + _vel_int.setZero(); + } + + /** + * Get the controllers output attitude setpoint + * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control. + * It needs to be executed by the attitude controller to achieve velocity and position tracking. + * @param attitude_setpoint reference to struct to fill up + */ + void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint, vehicle_attitude_s &v_att) const; + + /** + * All setpoints are set to NAN (uncontrolled). Timestampt zero. + */ + static const trajectory_setpoint_s empty_trajectory_setpoint; + +private: + // The range limits of the hover thrust configuration/estimate + static constexpr float HOVER_THRUST_MIN = 0.05f; + static constexpr float HOVER_THRUST_MAX = 0.9f; + + bool _inputValid(); + + void _positionControl(const float dt); ///< Position PI control + void _velocityControl(const float dt); ///< Velocity PID control + + // Gains + matrix::Vector3f _gain_pos_p; ///< Position control proportional gain + matrix::Vector3f _gain_pos_i; ///< Position control integral gain + matrix::Vector3f _gain_vel_p; ///< Velocity control proportional gain + matrix::Vector3f _gain_vel_i; ///< Velocity control integral gain + matrix::Vector3f _gain_vel_d; ///< Velocity control derivative gain + + // Limits + float _lim_vel{}; ///< Horizontal 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 _pos_int_lim{}; ///< Anti-windup for position control + float _vel_int_lim{}; ///< Anti-windup for velocity control + + // States + matrix::Vector3f _pos; /**< current position */ + matrix::Vector3f _pos_int; /**< integral term of the position controller */ + 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 */ + matrix::Quatf _att_q; /**< current attitude */ + 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 */ + matrix::Quatf _quat_sp; /**< desired attitude */ +}; diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScControlMathTest.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScControlMathTest.cpp new file mode 100644 index 0000000000..e9b6fba12f --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScControlMathTest.cpp @@ -0,0 +1,258 @@ +/**************************************************************************** + * + * 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 + +using namespace matrix; +using namespace ControlMath; + +TEST(ControlMathTest, LimitTiltUnchanged) +{ + Vector3f body = Vector3f(0.f, 0.f, 1.f).normalized(); + Vector3f body_before = body; + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); + + body = Vector3f(0.f, .1f, 1.f).normalized(); + body_before = body; + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, body_before); +} + +TEST(ControlMathTest, LimitTiltOpposite) +{ + Vector3f body = Vector3f(0.f, 0.f, -1.f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTiltAlmostOpposite) +{ + // This case doesn't trigger corner case handling but is very close to it + Vector3f body = Vector3f(0.001f, 0.f, -1.f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 45.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); +} + +TEST(ControlMathTest, LimitTilt45degree) +{ + Vector3f body = Vector3f(1.f, 0.f, 0.f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(M_SQRT1_2_F, 0, M_SQRT1_2_F)); + + body = Vector3f(0.f, 1.f, 0.f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 45.f); + EXPECT_EQ(body, Vector3f(0.f, M_SQRT1_2_F, M_SQRT1_2_F)); +} + +TEST(ControlMathTest, LimitTilt10degree) +{ + Vector3f body = Vector3f(1.f, 1.f, .1f).normalized(); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f); + float angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(body(0), body(1)); + + body = Vector3f(1, 2, .2f); + limitTilt(body, Vector3f(0.f, 0.f, 1.f), M_DEG_TO_RAD_F * 10.f); + angle = acosf(body.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_NEAR(angle * M_RAD_TO_DEG_F, 10.f, 1e-4f); + EXPECT_FLOAT_EQ(body.length(), 1.f); + EXPECT_FLOAT_EQ(2.f * body(0), body(1)); +} + +TEST(ControlMathTest, ThrottleAttitudeMapping) +{ + /* expected: zero roll, zero pitch, zero yaw, full thr mag + * reason: thrust pointing full upward */ + Vector3f thr{0.f, 0.f, -1.f}; + float yaw = 0.f; + vehicle_attitude_setpoint_s att{}; + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, 0.f); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, 0.f); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); + + /* expected: same as before but with 90 yaw + * reason: only yaw changed */ + yaw = M_PI_2_F; + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, 0.f); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); + + /* expected: same as before but roll 180 + * reason: thrust points straight down and order Euler + * order is: 1. roll, 2. pitch, 3. yaw */ + thr = Vector3f(0.f, 0.f, 1.f); + thrustToAttitude(thr, yaw, att); + EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F); + EXPECT_FLOAT_EQ(att.pitch_body, 0.f); + EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); +} + +TEST(ControlMathTest, ConstrainXYPriorities) +{ + const float max = 5.f; + // v0 already at max + Vector2f v0(max, 0.f); + Vector2f v1(v0(1), -v0(0)); + + Vector2f v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(0), max); + EXPECT_FLOAT_EQ(v_r(1), 0.f); + + // norm of v1 exceeds max but v0 is zero + v0.zero(); + v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(1), -max); + EXPECT_FLOAT_EQ(v_r(0), 0.f); + + v0 = Vector2f(.5f, .5f); + v1 = Vector2f(.5f, -.5f); + v_r = constrainXY(v0, v1, max); + const float diff = Vector2f(v_r - (v0 + v1)).length(); + EXPECT_FLOAT_EQ(diff, 0.f); + + // v0 and v1 exceed max and are perpendicular + v0 = Vector2f(4.f, 0.f); + v1 = Vector2f(0.f, -4.f); + v_r = constrainXY(v0, v1, max); + EXPECT_FLOAT_EQ(v_r(0), v0(0)); + EXPECT_GT(v_r(0), 0.f); + const float remaining = sqrtf(max * max - (v0(0) * v0(0))); + EXPECT_FLOAT_EQ(v_r(1), -remaining); +} + +TEST(ControlMathTest, CrossSphereLine) +{ + /* Testing 9 positions (+) around waypoints (o): + * + * Far + + + + * + * Near + + + + * On trajectory --+----o---------+---------o----+-- + * prev curr + * + * Expected targets (1, 2, 3): + * Far + + + + * + * + * On trajectory -------1---------2---------3------- + * + * + * Near + + + + * On trajectory -------o---1---------2-----3------- + * + * + * On trajectory --+----o----1----+--------2/3---+-- */ + const Vector3f prev = Vector3f(0.f, 0.f, 0.f); + const Vector3f curr = Vector3f(0.f, 0.f, 2.f); + Vector3f res; + bool retval = false; + + // on line, near, before previous waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, -.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 0.5f)); + + // on line, near, before target waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, 1.f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // on line, near, after target waypoint + retval = cross_sphere_line(Vector3f(0.f, 0.f, 2.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // near, before previous waypoint + retval = cross_sphere_line(Vector3f(0.f, .5f, -.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, .366025388f)); + + // near, before target waypoint + retval = cross_sphere_line(Vector3f(0.f, .5f, 1.f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.866025448f)); + + // near, after target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, .5f, 2.5f), 1.f, prev, curr, res); + EXPECT_TRUE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); + + // far, before previous waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, -.5f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f()); + + // far, before target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 1.f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.f)); + + // far, after target waypoint + retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 2.5f), 1.f, prev, curr, res); + EXPECT_FALSE(retval); + EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f)); +} + +TEST(ControlMathTest, addIfNotNan) +{ + float v = 1.f; + // regular addition + ControlMath::addIfNotNan(v, 2.f); + EXPECT_EQ(v, 3.f); + // addition is NAN and has no influence + ControlMath::addIfNotNan(v, NAN); + EXPECT_EQ(v, 3.f); + v = NAN; + // both summands are NAN + ControlMath::addIfNotNan(v, NAN); + EXPECT_TRUE(std::isnan(v)); + // regular value gets added to NAN and overwrites it + ControlMath::addIfNotNan(v, 3.f); + EXPECT_EQ(v, 3.f); +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScPositionControlTest.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScPositionControlTest.cpp new file mode 100644 index 0000000000..a9ce422ed5 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl/ScPositionControlTest.cpp @@ -0,0 +1,368 @@ +/**************************************************************************** + * + * 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 + +using namespace matrix; + +TEST(PositionControlTest, EmptySetpoint) +{ + ScPositionControl position_control; + + vehicle_attitude_setpoint_s attitude{}; + position_control.getAttitudeSetpoint(attitude); + EXPECT_FLOAT_EQ(attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f); + EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f); + EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); + EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); + EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); + EXPECT_EQ(attitude.reset_integral, false); + EXPECT_EQ(attitude.fw_control_yaw_wheel, false); +} + +class PositionControlBasicTest : public ::testing::Test +{ +public: + PositionControlBasicTest() + { + _position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f)); + _position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f)); + _position_control.setVelocityLimits(1.f, 1.f, 1.f); + _position_control.setThrustLimits(0.1f, MAXIMUM_THRUST); + _position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN); + _position_control.setTiltLimit(1.f); + _position_control.setHoverThrust(.5f); + } + + bool runController() + { + _position_control.setInputSetpoint(_input_setpoint); + const bool ret = _position_control.update(.1f); + _position_control.getAttitudeSetpoint(_attitude); + return ret; + } + + ScPositionControl _position_control; + trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint}; + vehicle_local_position_setpoint_s _output_setpoint{}; + vehicle_attitude_setpoint_s _attitude{}; + + static constexpr float MAXIMUM_THRUST = 0.9f; + static constexpr float HORIZONTAL_THRUST_MARGIN = 0.3f; +}; + +class PositionControlBasicDirectionTest : public PositionControlBasicTest +{ +public: + void checkDirection() + { + Vector3f thrust(_output_setpoint.thrust); + EXPECT_GT(thrust(0), 0.f); + EXPECT_GT(thrust(1), 0.f); + EXPECT_LT(thrust(2), 0.f); + + Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); + EXPECT_LT(body_z(0), 0.f); + EXPECT_LT(body_z(1), 0.f); + EXPECT_GT(body_z(2), 0.f); + } +}; + +TEST_F(PositionControlBasicDirectionTest, PositionDirection) +{ + Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.position); + EXPECT_TRUE(runController()); + checkDirection(); +} + +TEST_F(PositionControlBasicDirectionTest, VelocityDirection) +{ + Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.velocity); + EXPECT_TRUE(runController()); + checkDirection(); +} + +TEST_F(PositionControlBasicTest, TiltLimit) +{ + Vector3f(10.f, 10.f, 0.f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); + float angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_GT(angle, 0.f); + EXPECT_LE(angle, 1.f); + + _position_control.setTiltLimit(0.5f); + EXPECT_TRUE(runController()); + body_z = Quatf(_attitude.q_d).dcm_z(); + angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); + EXPECT_GT(angle, 0.f); + EXPECT_LE(angle, .50001f); + + _position_control.setTiltLimit(1.f); // restore original +} + +TEST_F(PositionControlBasicTest, VelocityLimit) +{ + Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy); + EXPECT_LE(velocity_xy.norm(), 1.f); + EXPECT_LE(abs(_output_setpoint.vz), 1.f); +} + +TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) +{ + // Given a setpoint that drives the controller into vertical and horizontal saturation + Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); + + // When you run it for one iteration + runController(); + Vector3f thrust(_output_setpoint.thrust); + + // Then the thrust vector length is limited by the maximum + EXPECT_FLOAT_EQ(thrust.norm(), MAXIMUM_THRUST); + + // Then the horizontal thrust is limited by its margin + EXPECT_FLOAT_EQ(thrust(0), HORIZONTAL_THRUST_MARGIN / sqrt(2.f)); + EXPECT_FLOAT_EQ(thrust(1), HORIZONTAL_THRUST_MARGIN / sqrt(2.f)); + EXPECT_FLOAT_EQ(thrust(2), + -sqrt(MAXIMUM_THRUST * MAXIMUM_THRUST - HORIZONTAL_THRUST_MARGIN * HORIZONTAL_THRUST_MARGIN)); + thrust.print(); + + // Then the collective thrust is limited by the maximum + EXPECT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST); + + // Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust + EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); + // TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore + // EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); +} + +TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) +{ + Vector3f(10.f, 0.f, 10.f).copyTo(_input_setpoint.position); + + runController(); + Vector3f thrust(_output_setpoint.thrust); + EXPECT_FLOAT_EQ(thrust.length(), 0.1f); + + EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f); + + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f); +} + +TEST_F(PositionControlBasicTest, FailsafeInput) +{ + _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f; + _input_setpoint.velocity[2] = .1f; + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f); + EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f); + EXPECT_LT(_output_setpoint.thrust[2], -.1f); + EXPECT_GT(_output_setpoint.thrust[2], -.5f); + EXPECT_GT(_attitude.thrust_body[2], -.5f); + EXPECT_LE(_attitude.thrust_body[2], -.1f); +} + +TEST_F(PositionControlBasicTest, IdleThrustInput) +{ + // High downwards acceleration to make sure there's no thrust + Vector3f(0.f, 0.f, 100.f).copyTo(_input_setpoint.acceleration); + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f); + EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); // minimum thrust +} + +TEST_F(PositionControlBasicTest, InputCombinationsPosition) +{ + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); + + EXPECT_TRUE(runController()); + EXPECT_FLOAT_EQ(_output_setpoint.x, .1f); + EXPECT_FLOAT_EQ(_output_setpoint.y, .2f); + EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); + EXPECT_FALSE(isnan(_output_setpoint.vx)); + EXPECT_FALSE(isnan(_output_setpoint.vy)); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} + +TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity) +{ + _input_setpoint.velocity[0] = .1f; + _input_setpoint.velocity[1] = .2f; + _input_setpoint.position[2] = .3f; // altitude + + EXPECT_TRUE(runController()); + EXPECT_TRUE(isnan(_output_setpoint.x)); + EXPECT_TRUE(isnan(_output_setpoint.y)); + EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); + EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f); + EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f); + EXPECT_FALSE(isnan(_output_setpoint.vz)); + EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); + EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); +} + +TEST_F(PositionControlBasicTest, SetpointValiditySimple) +{ + EXPECT_FALSE(runController()); + _input_setpoint.position[0] = .1f; + EXPECT_FALSE(runController()); + _input_setpoint.position[1] = .2f; + EXPECT_FALSE(runController()); + _input_setpoint.acceleration[2] = .3f; + EXPECT_TRUE(runController()); +} + +TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) +{ + // This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly + float *const setpoint_loop_access_map[] = {&_input_setpoint.position[0], &_input_setpoint.velocity[0], &_input_setpoint.acceleration[0], + &_input_setpoint.position[1], &_input_setpoint.velocity[1], &_input_setpoint.acceleration[1], + &_input_setpoint.position[2], &_input_setpoint.velocity[2], &_input_setpoint.acceleration[2] + }; + + for (int combination = 0; combination < 512; combination++) { + _input_setpoint = PositionControl::empty_trajectory_setpoint; + + for (int j = 0; j < 9; j++) { + if (combination & (1 << j)) { + // Set arbitrary finite value, some values clearly hit the limits to check these corner case combinations + *(setpoint_loop_access_map[j]) = static_cast(combination) / static_cast(j + 1); + } + } + + // Expect at least one setpoint per axis + const bool has_x_setpoint = ((combination & 7) != 0); + const bool has_y_setpoint = (((combination >> 3) & 7) != 0); + const bool has_z_setpoint = (((combination >> 6) & 7) != 0); + // Expect xy setpoints to come in pairs + const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7); + const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs; + + EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl + << "input" << std::endl + << "position " << _input_setpoint.position[0] << ", " + << _input_setpoint.position[1] << ", " << _input_setpoint.position[2] << std::endl + << "velocity " << _input_setpoint.velocity[0] << ", " + << _input_setpoint.velocity[1] << ", " << _input_setpoint.velocity[2] << std::endl + << "acceleration " << _input_setpoint.acceleration[0] << ", " + << _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl + << "output" << std::endl + << "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl + << "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl + << "acceleration " << _output_setpoint.acceleration[0] << ", " + << _output_setpoint.acceleration[1] << ", " << _output_setpoint.acceleration[2] << std::endl; + } +} + +TEST_F(PositionControlBasicTest, InvalidState) +{ + Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); + + PositionControlStates states{}; + states.position(0) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.velocity(0) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.position(0) = 0.f; + _position_control.setState(states); + EXPECT_FALSE(runController()); + + states.velocity(0) = 0.f; + states.acceleration(1) = NAN; + _position_control.setState(states); + EXPECT_FALSE(runController()); +} + + +TEST_F(PositionControlBasicTest, UpdateHoverThrust) +{ + // GIVEN: some hover thrust and 0 velocity change + const float hover_thrust = 0.6f; + _position_control.setHoverThrust(hover_thrust); + + Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); + + // WHEN: we run the controller + EXPECT_TRUE(runController()); + + // THEN: the output thrust equals the hover thrust + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); + + // HOWEVER WHEN: we set a new hover thrust through the update function + const float hover_thrust_new = 0.7f; + _position_control.updateHoverThrust(hover_thrust_new); + EXPECT_TRUE(runController()); + + // THEN: the integral is updated to avoid discontinuities and + // the output is still the same + EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); +} + +TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint) +{ + // GIVEN: the controller was ran with an invalid setpoint containing some valid values + _input_setpoint.position[0] = .1f; + _input_setpoint.position[1] = .2f; + // all z-axis setpoints stay NAN + EXPECT_FALSE(runController()); + + // WHEN: we run the controller with a valid setpoint + _input_setpoint = PositionControl::empty_trajectory_setpoint; + Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); + EXPECT_TRUE(runController()); + + // THEN: the integral did not wind up and produce unexpected deviation + EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); + EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp new file mode 100644 index 0000000000..5dd63f23b5 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.cpp @@ -0,0 +1,433 @@ +/**************************************************************************** + * + * 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 "SpacecraftPositionControl.hpp" + +#include +#include +#include "PositionControl/ControlMath.hpp" + +using namespace matrix; + +SpacecraftPositionControl::SpacecraftPositionControl(ModuleParams *parent) : ModuleParams(parent), + _vehicle_attitude_setpoint_pub(ORB_ID(vehicle_attitude_setpoint)), + _vel_x_deriv(this, "VELD"), + _vel_y_deriv(this, "VELD"), + _vel_z_deriv(this, "VELD") +{ + updateParams(); +} + +void SpacecraftPositionControl::updateParams() +{ + // check for parameter updates + if (_parameter_update_sub.updated()) { + // 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.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_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)); + } + + 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_vel_all.get() >= 0.f) { + float all_vel = _param_mpc_vel_all.get(); + num_changed += _param_mpc_vel_manual.commit_no_notification(all_vel); + num_changed += _param_mpc_vel_cruise.commit_no_notification(all_vel); + num_changed += _param_mpc_vel_max.commit_no_notification(all_vel); + } + + if (num_changed > 0) { + param_notify_changes(); + } + + // Set PI and PID gains, as well as anti-windup limits + _control.setPositionGains( + Vector3f(_param_mpc_pos_p.get(), _param_mpc_pos_p.get(), _param_mpc_pos_p.get()), + Vector3f(_param_mpc_pos_i.get(), _param_mpc_pos_i.get(), _param_mpc_pos_i.get())); + _control.setPositionIntegralLimits(_param_mpc_pos_i_lim.get()); + _control.setVelocityGains( + Vector3f(_param_mpc_vel_p_acc.get(), _param_mpc_vel_p_acc.get(), _param_mpc_vel_p_acc.get()), + Vector3f(_param_mpc_vel_i_acc.get(), _param_mpc_vel_i_acc.get(), _param_mpc_vel_i_acc.get()), + Vector3f(_param_mpc_vel_d_acc.get(), _param_mpc_vel_d_acc.get(), _param_mpc_vel_d_acc.get())); + _control.setVelocityIntegralLimits(_param_mpc_vel_i_lim.get()); + + // Check that the design parameters are inside the absolute maximum constraints + if (_param_mpc_vel_cruise.get() > _param_mpc_vel_max.get()) { + _param_mpc_vel_cruise.set(_param_mpc_vel_max.get()); + _param_mpc_vel_cruise.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed\t"); + /* EVENT + * @description SPC_VEL_CRUISE is set to {1:.0}. + */ + events::send(events::ID("sc_pos_ctrl_cruise_set"), events::Log::Warning, + "Cruise speed has been constrained by maximum speed", _param_mpc_vel_max.get()); + } + + if (_param_mpc_vel_manual.get() > _param_mpc_vel_max.get()) { + _param_mpc_vel_manual.set(_param_mpc_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 SPC_VEL_MANUAL is set to {1:.0}. + */ + events::send(events::ID("sc_pos_ctrl_man_vel_set"), events::Log::Warning, + "Manual speed has been constrained by maximum speed", _param_mpc_vel_max.get()); + } + + yaw_rate = math::radians(_param_mpc_man_y_max.get()); + } +} + +PositionControlStates SpacecraftPositionControl::set_vehicle_states(const vehicle_local_position_s + &vehicle_local_position, const vehicle_attitude_s &vehicle_attitude) +{ + PositionControlStates states; + + const Vector2f position_xy(vehicle_local_position.x, vehicle_local_position.y); + + // only set position states if valid and finite + if (vehicle_local_position.xy_valid && position_xy.isAllFinite()) { + states.position.xy() = position_xy; + + } else { + states.position(0) = states.position(1) = NAN; + } + + if (PX4_ISFINITE(vehicle_local_position.z) && vehicle_local_position.z_valid) { + states.position(2) = vehicle_local_position.z; + + } else { + states.position(2) = NAN; + } + + const Vector2f velocity_xy(vehicle_local_position.vx, vehicle_local_position.vy); + + if (vehicle_local_position.v_xy_valid && velocity_xy.isAllFinite()) { + states.velocity.xy() = velocity_xy; + states.acceleration(0) = _vel_x_deriv.update(velocity_xy(0)); + states.acceleration(1) = _vel_y_deriv.update(velocity_xy(1)); + + } else { + states.velocity(0) = states.velocity(1) = NAN; + states.acceleration(0) = states.acceleration(1) = NAN; + + // reset derivatives to prevent acceleration spikes when regaining velocity + _vel_x_deriv.reset(); + _vel_y_deriv.reset(); + } + + if (PX4_ISFINITE(vehicle_local_position.vz) && vehicle_local_position.v_z_valid) { + states.velocity(2) = vehicle_local_position.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(); + } + + if (PX4_ISFINITE(vehicle_attitude.q[0]) && PX4_ISFINITE(vehicle_attitude.q[1]) && PX4_ISFINITE(vehicle_attitude.q[2]) + && PX4_ISFINITE(vehicle_attitude.q[3])) { + states.quaternion = Quatf(vehicle_attitude.q); + + } else { + states.quaternion = Quatf(); + } + + return states; +} + +void SpacecraftPositionControl::updatePositionControl() +{ + vehicle_local_position_s vehicle_local_position; + vehicle_attitude_s v_att; + + if (_local_pos_sub.update(&vehicle_local_position)) { + const float dt = + math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); + _time_stamp_last_loop = vehicle_local_position.timestamp_sample; + + // set _dt in controllib Block for BlockDerivative + setDt(dt); + + if (_vehicle_control_mode_sub.updated()) { + const bool previous_position_control_enabled = _vehicle_control_mode.flag_control_position_enabled; + + if (_vehicle_control_mode_sub.update(&_vehicle_control_mode)) { + if (!previous_position_control_enabled && _vehicle_control_mode.flag_control_position_enabled) { + _time_position_control_enabled = _vehicle_control_mode.timestamp; + + } else if (previous_position_control_enabled && !_vehicle_control_mode.flag_control_position_enabled) { + // clear existing setpoint when controller is no longer active + _setpoint = ScPositionControl::empty_trajectory_setpoint; + } + } + } + + // TODO: check if setpoint is different than the previous one and reset integral then + // _control.resetIntegral(); + _trajectory_setpoint_sub.update(&_setpoint); + _vehicle_attitude_sub.update(&v_att); + + // adjust existing (or older) setpoint with any EKF reset deltas + if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) { + if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) { + _setpoint.velocity[0] += vehicle_local_position.delta_vxy[0]; + _setpoint.velocity[1] += vehicle_local_position.delta_vxy[1]; + } + + if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) { + _setpoint.velocity[2] += vehicle_local_position.delta_vz; + } + + if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) { + _setpoint.position[0] += vehicle_local_position.delta_xy[0]; + _setpoint.position[1] += vehicle_local_position.delta_xy[1]; + } + + if (vehicle_local_position.z_reset_counter != _z_reset_counter) { + _setpoint.position[2] += vehicle_local_position.delta_z; + } + + if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) { + _setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading); + } + } + + if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) { + _vel_x_deriv.reset(); + _vel_y_deriv.reset(); + } + + if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) { + _vel_z_deriv.reset(); + } + + // save latest reset counters + _vxy_reset_counter = vehicle_local_position.vxy_reset_counter; + _vz_reset_counter = vehicle_local_position.vz_reset_counter; + _xy_reset_counter = vehicle_local_position.xy_reset_counter; + _z_reset_counter = vehicle_local_position.z_reset_counter; + _heading_reset_counter = vehicle_local_position.heading_reset_counter; + + PositionControlStates states{set_vehicle_states(vehicle_local_position, v_att)}; + + poll_manual_setpoint(dt, vehicle_local_position, v_att); + + if (_vehicle_control_mode.flag_control_position_enabled) { + // set failsafe setpoint if there hasn't been a new + // trajectory setpoint since position control started + if ((_setpoint.timestamp < _time_position_control_enabled) + && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) { + PX4_INFO("Setpoint time: %f, Vehicle local pos time: %f, Pos Control Enabled time: %f", + (double)_setpoint.timestamp, (double)vehicle_local_position.timestamp_sample, + (double)_time_position_control_enabled); + _setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, false); + } + } + + if (_vehicle_control_mode.flag_control_position_enabled + && (_setpoint.timestamp >= _time_position_control_enabled)) { + + _control.setThrustLimit(_param_mpc_thr_max.get()); + + _control.setVelocityLimits(_param_mpc_vel_max.get()); + + _control.setInputSetpoint(_setpoint); + + _control.setState(states); + + // Run position control + if (!_control.update(dt)) { + _control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, true)); + _control.setVelocityLimits(_param_mpc_vel_max.get()); + _control.update(dt); + } + + // Publish attitude setpoint output + vehicle_attitude_setpoint_s attitude_setpoint{}; + _control.getAttitudeSetpoint(attitude_setpoint, v_att); + // PX4_INFO("States: %f %f %f / %f %f %f", (double)states.position(0), (double)states.position(1), + // (double)states.position(2), (double)states.velocity(0), (double)states.velocity(1), + // (double)states.velocity(2)); + // PX4_INFO("Setpoint: %f %f %f / %f %f %f", (double)_setpoint.position[0], (double)_setpoint.position[1], + // (double)_setpoint.position[2], (double)_setpoint.velocity[0], (double)_setpoint.velocity[1], + // (double)_setpoint.velocity[2]); + // PX4_INFO("Control input: %f %f %f / %f %f %f %f", (double)attitude_setpoint.thrust_body[0], (double)attitude_setpoint.thrust_body[1], + // (double)attitude_setpoint.thrust_body[2], (double)attitude_setpoint.q_d[0], (double)attitude_setpoint.q_d[1], + // (double)attitude_setpoint.q_d[2], (double)attitude_setpoint.q_d[3]); + attitude_setpoint.timestamp = hrt_absolute_time(); + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + + // publish setpoint + publishLocalPositionSetpoint(attitude_setpoint); + } + } +} + +void SpacecraftPositionControl::publishLocalPositionSetpoint(vehicle_attitude_setpoint_s &_att_sp) +{ + // complete the setpoint data structure + vehicle_local_position_setpoint_s local_position_setpoint{}; + local_position_setpoint.timestamp = hrt_absolute_time(); + + local_position_setpoint.x = _setpoint.position[0]; + local_position_setpoint.y = _setpoint.position[1]; + local_position_setpoint.z = _setpoint.position[2]; + local_position_setpoint.yaw = NAN; + local_position_setpoint.yawspeed = NAN; + local_position_setpoint.vx = _setpoint.velocity[0]; + local_position_setpoint.vy = _setpoint.velocity[1]; + local_position_setpoint.vz = _setpoint.velocity[2]; + local_position_setpoint.acceleration[0] = _setpoint.acceleration[0]; + local_position_setpoint.acceleration[1] = _setpoint.acceleration[1]; + local_position_setpoint.acceleration[2] = _setpoint.acceleration[2]; + local_position_setpoint.thrust[0] = _att_sp.thrust_body[0]; + local_position_setpoint.thrust[1] = _att_sp.thrust_body[1]; + local_position_setpoint.thrust[2] = _att_sp.thrust_body[2]; + _local_pos_sp_pub.publish(local_position_setpoint); +} + +void SpacecraftPositionControl::poll_manual_setpoint(const float dt, + const vehicle_local_position_s &vehicle_local_position, + const vehicle_attitude_s &_vehicle_att) +{ + if (_vehicle_control_mode.flag_control_manual_enabled && _vehicle_control_mode.flag_armed) { + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { + + if (!_vehicle_control_mode.flag_control_climb_rate_enabled && + !_vehicle_control_mode.flag_control_offboard_enabled) { + + if (_vehicle_control_mode.flag_control_attitude_enabled && + _vehicle_control_mode.flag_control_position_enabled) { + // We are in Stabilized mode + // Generate position setpoints + if (!stabilized_pos_sp_initialized) { + // Initialize position setpoint + target_pos_sp = Vector3f(vehicle_local_position.x, vehicle_local_position.y, + vehicle_local_position.z); + + const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi(); + _manual_yaw_sp = vehicle_yaw; + stabilized_pos_sp_initialized = true; + } + + // Update velocity setpoint + Vector3f target_vel_sp = Vector3f(_manual_control_setpoint.pitch, _manual_control_setpoint.roll, 0.0); + target_pos_sp = target_pos_sp + target_vel_sp * dt; + + // Update _setpoint + _setpoint.position[0] = target_pos_sp(0); + _setpoint.position[1] = target_pos_sp(1); + _setpoint.position[2] = target_pos_sp(2); + + _setpoint.velocity[0] = target_vel_sp(0); + _setpoint.velocity[1] = target_vel_sp(1); + _setpoint.velocity[2] = target_vel_sp(2); + + // Generate attitude setpoints + float yaw_sp_move_rate = 0.0; + + if (_manual_control_setpoint.throttle > -0.9f) { + yaw_sp_move_rate = _manual_control_setpoint.yaw * yaw_rate; + } + + _manual_yaw_sp = wrap_pi(_manual_yaw_sp + yaw_sp_move_rate * dt); + const float roll_body = 0.0; + const float pitch_body = 0.0; + + Quatf q_sp(Eulerf(roll_body, pitch_body, _manual_yaw_sp)); + q_sp.copyTo(_setpoint.attitude); + + _setpoint.timestamp = hrt_absolute_time(); + + } else { + // We are in Manual mode + stabilized_pos_sp_initialized = false; + } + + } else { + stabilized_pos_sp_initialized = false; + } + + _manual_setpoint_last_called = hrt_absolute_time(); + } + } +} + +trajectory_setpoint_s SpacecraftPositionControl::generateFailsafeSetpoint(const hrt_abstime &now, + const PositionControlStates &states, bool warn) +{ + // rate limit the warnings + warn = warn && (now - _last_warn) > 2_s; + + if (warn) { + PX4_WARN("invalid setpoints"); + _last_warn = now; + } + + trajectory_setpoint_s failsafe_setpoint = ScPositionControl::empty_trajectory_setpoint; + failsafe_setpoint.timestamp = now; + + failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = failsafe_setpoint.velocity[2] = 0.f; + + if (warn) { + PX4_WARN("Failsafe: stop and wait"); + } + + return failsafe_setpoint; +} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp new file mode 100644 index 0000000000..a58531689e --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/SpacecraftPositionControl.hpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * 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 +#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 SpacecraftPositionControl : public ModuleParams +{ +public: + SpacecraftPositionControl(ModuleParams *parent); + ~SpacecraftPositionControl() = default; + + bool updatePositionControl(); + +protected: + /** + * Update our local parameter cache. + */ + void updateParams(bool force); + +private: + + orb_advert_t _mavlink_log_pub{nullptr}; + + 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::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< vehicle local position */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + + hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ + hrt_abstime _time_position_control_enabled{0}; + hrt_abstime _manual_setpoint_last_called{0}; + + trajectory_setpoint_s _setpoint{ScPositionControl::empty_trajectory_setpoint}; + vehicle_control_mode_s _vehicle_control_mode{}; + manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ + + DEFINE_PARAMETERS( + // Position Control + (ParamFloat) _param_mpc_pos_p, + (ParamFloat) _param_mpc_pos_i, + (ParamFloat) _param_mpc_pos_i_lim, + (ParamFloat) _param_mpc_vel_p_acc, + (ParamFloat) _param_mpc_vel_i_acc, + (ParamFloat) _param_mpc_vel_i_lim, + (ParamFloat) _param_mpc_vel_d_acc, + (ParamFloat) _param_mpc_vel_all, + (ParamFloat) _param_mpc_vel_max, + (ParamFloat) _param_mpc_vel_cruise, + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_sys_vehicle_resp, + (ParamFloat) _param_mpc_acc, + (ParamFloat) _param_mpc_acc_max, + (ParamFloat) _param_mpc_man_y_max, + (ParamFloat) _param_mpc_man_y_tau, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_thr_max + ); + + 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 */ + + matrix::Vector3f target_pos_sp; + float yaw_rate; + bool stabilized_pos_sp_initialized{false}; + + ScPositionControl _control; /**< class for core PID position control */ + + hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */ + + /** Timeout in us for trajectory data to get considered invalid */ + static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; + + 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}; + + // Manual setpoints on yaw and reset + bool _reset_yaw_sp{true}; + float _manual_yaw_sp{0.f}; + float _throttle_control{0.f}; + float _yaw_control{0.f}; + + /** + * Check for validity of positon/velocity states. + */ + PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos, const vehicle_attitude_s &att); + + /** + * Check for manual setpoints. + */ + void poll_manual_setpoint(const float dt, const vehicle_local_position_s + &vehicle_local_position, const vehicle_attitude_s &_vehicle_att); + + /** + * @brief publishes target setpoint. + * + */ + void publishLocalPositionSetpoint(vehicle_attitude_setpoint_s &_att_sp); + + /** + * Generate setpoint to bridge no executable setpoint being available. + * Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid. + * This should only happen briefly when transitioning and never during mode operation or by design. + */ + trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn); +}; diff --git a/src/modules/spacecraft/spacecraft_position_params.c b/src/modules/spacecraft/spacecraft_position_params.c new file mode 100644 index 0000000000..a920fb2f6f --- /dev/null +++ b/src/modules/spacecraft/spacecraft_position_params.c @@ -0,0 +1,294 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +/** + * Proportional gain for position error + * + * Defined as corrective velocity in m/s per m position error + * + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_POS_P, 0.2f); + +/** + * Integral gain for position error + * + * Defined as corrective velocity in m/s per m velocity error + * + * @min 0 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_POS_I, 0.f); + +/** + * Integral limit for position error + * + * Defined as corrective velocity in m/s per m velocity error + * + * @min 0 + * @max 5 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_POS_I_LIM, 1.f); + +/** + * Proportional gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 0 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_P, 6.55f); + +/** + * Integral gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 0 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_I, 0.f); + +/** + * Integral limit for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 0 + * @max 5 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_I_LIM, 1.f); + +/** + * Derivative gain for velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 0.0 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_D, 0.0f); + +/** + * Maximum velocity + * + * Absolute maximum for all velocity controlled modes. + * Any higher value is truncated. + * + * @unit m/s + * @min 0 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_MAX, 12.f); + +/** + * Overall Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_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 Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_ALL, -10.f); + +/** + * Cruising elocity setpoint in autonomous modes + * + * @unit m/s + * @min 3 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_CRUISE, 10.f); + +/** + * Maximum velocity setpoint in Position mode + * + * @unit m/s + * @min 3 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VEL_MANUAL, 10.f); + +/** + * Maximum collective thrust + * + * Limit allowed thrust + * + * @unit norm + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_THR_MAX, 1.f); + +/** + * Acceleration for autonomous and for manual modes + * + * When piloting manually, this parameter is only used in MPC_POS_MODE 4. + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_ACC, 3.f); + +/** + * Maximum accelaration in autonomous modes + * + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_ACC_MAX, 5.f); + +/** + * Jerk limit in autonomous modes + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother vehicle motions but also limited agility. + * + * @unit m/s^3 + * @min 1 + * @max 80 + * @decimal 1 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_JERK_AUTO, 4.f); + +PARAM_DEFINE_FLOAT(SPC_VEHICLE_RESP, 0.5f); +/** + * Maximum jerk in Position/Altitude mode + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother motions but limits agility + * (how fast it can change directions or break). + * + * Setting this to the maximum value essentially disables the limit. + * + * Only used with smooth MPC_POS_MODE 3 and 4. + * + * @unit m/s^3 + * @min 0.5 + * @max 500 + * @decimal 0 + * @increment 1 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_JERK_MAX, 8.f); + +/** + * Max manual yaw rate for Stabilized, Altitude, Position mode + * + * @unit deg/s + * @min 0 + * @max 400 + * @decimal 0 + * @increment 10 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_MAN_Y_MAX, 150.f); + +/** + * Manual yaw rate input filter time constant + * + * Not used in Stabilized mode + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0 + * @max 5 + * @decimal 2 + * @increment 0.01 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_MAN_Y_TAU, 0.08f); + +/** + * Numerical velocity derivative low pass cutoff frequency + * + * @unit Hz + * @min 0 + * @max 10 + * @decimal 1 + * @increment 0.5 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SPC_VELD_LP, 5.0f); +