diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.cpp new file mode 100644 index 0000000000..7c335485ea --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 AttitudeControl.cpp + */ + +#include + +#include + +using namespace matrix; + +void ScAttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain) +{ + _proportional_gain = proportional_gain; +} + +matrix::Vector3f ScAttitudeControl::update(const Quatf &q) const +{ + Quatf qd = _attitude_setpoint_q; + + // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch + const Vector3f e_z = q.dcm_z(); + const Vector3f e_z_d = qd.dcm_z(); + Quatf qd_red(e_z, e_z_d); + + if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) { + // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction, + // full attitude control anyways generates no yaw input and directly takes the combination of + // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable. + qd_red = qd; + + } else { + // transform rotation from current to desired thrust vector into a world frame reduced desired attitude + qd_red *= q; + } + + // mix full and reduced desired attitude + Quatf q_mix = qd_red.inversed() * qd; + q_mix.canonicalize(); + + // catch numerical problems with the domain of acosf and asinf + q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); + q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); + qd = qd_red * Quatf(q_mix(0), 0, 0, q_mix(3)); + + // quaternion attitude control law, qe is rotation from q to qd + const Quatf qe = q.inversed() * qd; + + // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) + // also taking care of the antipodal unit quaternion ambiguity + const Vector3f eq = 2.f * qe.canonical().imag(); + + // calculate angular rates setpoint + Vector3f rate_setpoint = eq.emult(_proportional_gain); + + // limit rates + for (int i = 0; i < 3; i++) { + rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i)); + } + + return rate_setpoint; +} diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.hpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.hpp new file mode 100644 index 0000000000..3d702055b8 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.hpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 AttitudeControl.hpp + * + * A quaternion based attitude controller. + * + * @author Matthias Grob + * + * Publication documenting the implemented Quaternion Attitude Control: + * Nonlinear Quadrocopter Attitude Control (2013) + * by Dario Brescianini, Markus Hehn and Raffaello D'Andrea + * Institute for Dynamic Systems and Control (IDSC), ETH Zurich + * + * https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf + */ + +#pragma once + +#include +#include + +class ScAttitudeControl +{ +public: + ScAttitudeControl() = default; + ~ScAttitudeControl() = default; + + /** + * Set proportional attitude control gain + * @param proportional_gain 3D vector containing gains for roll, pitch, yaw + */ + void setProportionalGain(const matrix::Vector3f &proportional_gain); + + /** + * Set hard limit for output rate setpoints + * @param rate_limit [rad/s] 3D vector containing limits for roll, pitch, yaw + */ + void setRateLimit(const matrix::Vector3f &rate_limit) { _rate_limit = rate_limit; } + + /** + * Set a new attitude setpoint replacing the one tracked before + * @param qd desired vehicle attitude setpoint + */ + void setAttitudeSetpoint(const matrix::Quatf &qd) + { + _attitude_setpoint_q = qd; + _attitude_setpoint_q.normalize(); + } + + /** + * Adjust last known attitude setpoint by a delta rotation + * Optional use to avoid glitches when attitude estimate reference e.g. heading changes. + * @param q_delta delta rotation to apply + */ + void adaptAttitudeSetpoint(const matrix::Quatf &q_delta) + { + _attitude_setpoint_q = q_delta * _attitude_setpoint_q; + _attitude_setpoint_q.normalize(); + } + + /** + * Run one control loop cycle calculation + * @param q estimation of the current vehicle attitude unit quaternion + * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller + */ + matrix::Vector3f update(const matrix::Quatf &q) const; + +private: + matrix::Vector3f _proportional_gain; + matrix::Vector3f _rate_limit; + + matrix::Quatf _attitude_setpoint_q; ///< latest known attitude setpoint e.g. from position control +}; diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControlMath.hpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControlMath.hpp new file mode 100644 index 0000000000..fe0f760ed4 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControlMath.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 AttitudeControlMath.hpp + */ + +#pragma once + +#include + +namespace AttitudeControlMath +{ +/** + * Rotate a tilt quaternion (without yaw rotation) such that when rotated by a yaw setpoint + * the resulting tilt is the same as if it was rotated by the current yaw of the vehicle + * @param q_sp_tilt pure tilt quaternion (yaw = 0) that needs to be corrected + * @param q_att current attitude of the vehicle + * @param q_sp_yaw pure yaw quaternion of the desired yaw setpoint + */ +void inline correctTiltSetpointForYawError(matrix::Quatf &q_sp_tilt, const matrix::Quatf &q_att, + const matrix::Quatf &q_sp_yaw) +{ + const matrix::Vector3f z_unit(0.f, 0.f, 1.f); + + // Extract yaw from the current attitude + const matrix::Vector3f att_z = q_att.dcm_z(); + const matrix::Quatf q_tilt(z_unit, att_z); + const matrix::Quatf q_yaw = q_tilt.inversed() * q_att; // This is not euler yaw + + // Find the quaternion that creates a tilt aligned with the body frame + // when rotated by the yaw setpoint + // To do so, solve q_yaw * q_tilt_ne = q_sp_yaw * q_sp_rp_compensated + const matrix::Quatf q_sp_rp_compensated = q_sp_yaw.inversed() * q_yaw * q_sp_tilt; + + // Extract the corrected tilt + const matrix::Vector3f att_sp_z = q_sp_rp_compensated.dcm_z(); + q_sp_tilt = matrix::Quatf(z_unit, att_sp_z); +} +} diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt new file mode 100644 index 0000000000..ab68cd5a74 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/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(SpacecraftAttitudeControl + AttitudeControl.cpp + AttitudeControl.hpp + AttitudeControlMath.hpp +) +target_compile_options(SpacecraftAttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(SpacecraftAttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +# TODO: Add unit tests +# px4_add_unit_gtest(SRC ScAttitudeControlTest.cpp LINKLIBS SpacecraftAttitudeControl) +# px4_add_unit_gtest(SRC ScAttitudeControlMathTest.cpp LINKLIBS SpacecraftAttitudeControl) diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlMathTest.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlMathTest.cpp new file mode 100644 index 0000000000..b247fce074 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlMathTest.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 "AttitudeControlMath.hpp" + +using namespace matrix; +using namespace AttitudeControlMath; + +static const Vector3f z_unit(0.f, 0.f, 1.f); + +TEST(AttitudeControlMath, tiltCorrectionNoError) +{ + // GIVEN: some desired (non yaw-rotated) tilt setpoint + Quatf q_tilt_sp_ne(z_unit, Vector3f(-0.3, 0.1, 0.7)); + + // AND: a desired yaw setpoint + const Quatf q_sp_yaw = AxisAnglef(z_unit, -1.23f); + + // WHEN: the current yaw error is zero (regardless of the tilt) + const Quatf q = q_sp_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f)); + const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne; + correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw); + + // THEN: the tilt setpoint is unchanged + EXPECT_TRUE(isEqual(q_tilt_sp_ne_before, q_tilt_sp_ne)); +} + +TEST(AttitudeControlMath, tiltCorrectionYaw180) +{ + // GIVEN: some desired (non yaw-rotated) tilt setpoint and a desired yaw setpoint + Quatf q_tilt_sp_ne(z_unit, Vector3f(-0.3, 0.1, 0.7)); + const Quatf q_sp_yaw = AxisAnglef(z_unit, -M_PI_F / 2.f); + + // WHEN: there is a yaw error of 180 degrees + const Quatf q_yaw = Quatf(AxisAnglef(z_unit, M_PI_F / 2.f)); + const Quatf q = q_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f)); + const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne; + correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw); + + // THEN: the tilt is reversed (the corrected tilt angle is the same but the axis of rotation is opposite) + EXPECT_FLOAT_EQ(AxisAnglef(q_tilt_sp_ne_before).angle(), AxisAnglef(q_tilt_sp_ne).angle()); + EXPECT_TRUE(isEqual(AxisAnglef(q_tilt_sp_ne_before).axis(), -AxisAnglef(q_tilt_sp_ne).axis())); +} + +TEST(AttitudeControlMath, tiltCorrection) +{ + // GIVEN: some desired (non yaw-rotated) tilt setpoint and a desired yaw setpoint + Quatf q_tilt_sp_ne(z_unit, Vector3f(0.5, -0.1, 0.7)); + const Quatf q_sp_yaw = AxisAnglef(z_unit, -1.23f); + + // WHEN: there is a some yaw error + const Quatf q_yaw = Quatf(AxisAnglef(z_unit, 3.1f)); + const Quatf q = q_yaw * Quatf(z_unit, Vector3f(0.1f, -0.2f, 1.f)); + const Quatf q_tilt_sp_ne_before = q_tilt_sp_ne; + correctTiltSetpointForYawError(q_tilt_sp_ne, q, q_sp_yaw); + + // THEN: the tilt vector obtained by rotating the corrected tilt by the yaw setpoint is the same as + // the one obtained by rotating the initial tilt by the current yaw of the vehicle + EXPECT_TRUE(isEqual((q_sp_yaw * q_tilt_sp_ne).dcm_z(), (q_yaw * q_tilt_sp_ne_before).dcm_z())); +} diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlTest.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlTest.cpp new file mode 100644 index 0000000000..c7c20c5a62 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlTest.cpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * 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(ScAttitudeControlTest, AllZeroCase) +{ + ScAttitudeControl attitude_control; + Vector3f rate_setpoint = attitude_control.update(Quatf()); + EXPECT_EQ(rate_setpoint, Vector3f()); +} + +class ScAttitudeControlConvergenceTest : public ::testing::Test +{ +public: + ScAttitudeControlConvergenceTest() + { + _attitude_control.setProportionalGain(Vector3f(.5f, .6f, .3f)); + _attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f)); + } + + void checkConvergence() + { + int i; // need function scope to check how many steps + Vector3f rate_setpoint(1000.f, 1000.f, 1000.f); + + _attitude_control.setAttitudeSetpoint(_quat_goal); + + for (i = 100; i > 0; i--) { + // run attitude control to get rate setpoints + const Vector3f rate_setpoint_new = _attitude_control.update(_quat_state); + // rotate the simulated state quaternion according to the rate setpoint + _quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new)); + _quat_state = -_quat_state; // produce intermittent antipodal quaternion states to test against unwinding problem + + // expect the error and hence also the output to get smaller with each iteration + if (rate_setpoint_new.norm() >= rate_setpoint.norm()) { + break; + } + + rate_setpoint = rate_setpoint_new; + } + + EXPECT_EQ(_quat_state.canonical(), _quat_goal.canonical()); + // it shouldn't have taken longer than an iteration timeout to converge + EXPECT_GT(i, 0); + } + + ScAttitudeControl _attitude_control; + Quatf _quat_state; + Quatf _quat_goal; +}; + +TEST_F(ScAttitudeControlConvergenceTest, AttitudeControlConvergence) +{ + const int inputs = 8; + + const Quatf QArray[inputs] = { + Quatf(), + Quatf(0, 1, 0, 0), + Quatf(0, 0, 1, 0), + Quatf(0, 0, 0, 1), + Quatf(0.698f, 0.024f, -0.681f, -0.220f), + Quatf(-0.820f, -0.313f, 0.225f, -0.423f), + Quatf(0.599f, -0.172f, 0.755f, -0.204f), + Quatf(0.216f, -0.662f, 0.290f, -0.656f) + }; + + for (int i = 0; i < inputs; i++) { + for (int j = 0; j < inputs; j++) { + printf("--- Input combination: %d %d\n", i, j); + _quat_state = QArray[i]; + _quat_goal = QArray[j]; + _quat_state.normalize(); + _quat_goal.normalize(); + checkConvergence(); + } + } +} + +TEST(ScAttitudeControlTest, YawWeightScaling) +{ + // GIVEN: default tuning and pure yaw turn command + ScAttitudeControl attitude_control; + const float yaw_gain = 2.8f; + const float yaw_sp = .1f; + Quatf pure_yaw_attitude(cosf(yaw_sp / 2.f), 0, 0, sinf(yaw_sp / 2.f)); + attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain)); + attitude_control.setRateLimit(Vector3f(1000.f, 1000.f, 1000.f)); + attitude_control.setAttitudeSetpoint(pure_yaw_attitude); + + // WHEN: we run one iteration of the controller + Vector3f rate_setpoint = attitude_control.update(Quatf()); + + // THEN: no actuation in roll, pitch + EXPECT_EQ(Vector2f(rate_setpoint), Vector2f()); + // THEN: actuation error * gain in yaw + EXPECT_NEAR(rate_setpoint(2), yaw_sp * yaw_gain, 1e-4f); + + // GIVEN: additional corner case of zero yaw weight + attitude_control.setProportionalGain(Vector3f(6.5f, 6.5f, yaw_gain)); + // WHEN: we run one iteration of the controller + rate_setpoint = attitude_control.update(Quatf()); + // THEN: no actuation (also no NAN) + EXPECT_EQ(rate_setpoint, Vector3f()); +} diff --git a/src/modules/spacecraft/SpacecraftHandler.cpp b/src/modules/spacecraft/SpacecraftHandler.cpp index 92cdf86655..c397dbd3f6 100644 --- a/src/modules/spacecraft/SpacecraftHandler.cpp +++ b/src/modules/spacecraft/SpacecraftHandler.cpp @@ -34,16 +34,50 @@ /** * @file SpacecraftHandler.cpp * - * Control allocator. + * Spacecraft control handler. * - * @author Julien Lecoeur + * @author Pedro Roque */ #include "SpacecraftHandler.hpp" + +using namespace time_literals; + +SpacecraftHandler::SpacecraftHandler() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); +} + +bool SpacecraftHandler::init() +{ + ScheduleOnInterval(4_ms); // 250 Hz + return true; +} + int SpacecraftHandler::task_spawn(int argc, char *argv[]) { - return 0; + SpacecraftHandler *instance = new SpacecraftHandler(); + + 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 SpacecraftHandler::print_status() @@ -75,6 +109,7 @@ int SpacecraftHandler::print_usage(const char *reason) PRINT_MODULE_USAGE_NAME("spacecraft", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND("status"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/spacecraft/SpacecraftHandler.hpp b/src/modules/spacecraft/SpacecraftHandler.hpp index 8fc954dc71..28380d2ca4 100644 --- a/src/modules/spacecraft/SpacecraftHandler.hpp +++ b/src/modules/spacecraft/SpacecraftHandler.hpp @@ -68,7 +68,7 @@ public: SpacecraftHandler(); - virtual ~SpacecraftHandler(); + ~SpacecraftHandler() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -82,6 +82,12 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; -private: /**< loop duration performance counter */ + bool init(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; }; diff --git a/src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt new file mode 100644 index 0000000000..9b197ae5b1 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/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(SpacecraftPositionControl + ControlMath.cpp + ControlMath.hpp + PositionControl.cpp + PositionControl.hpp +) +target_include_directories(SpacecraftPositionControl 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/ControlMath.cpp b/src/modules/spacecraft/SpacecraftPositionControl/ControlMath.cpp new file mode 100644 index 0000000000..d2228d5597 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/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/ControlMath.hpp b/src/modules/spacecraft/SpacecraftPositionControl/ControlMath.hpp new file mode 100644 index 0000000000..c2d9b8b7f8 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/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.cpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl.cpp new file mode 100644 index 0000000000..6d9ea12767 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/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.hpp b/src/modules/spacecraft/SpacecraftPositionControl/PositionControl.hpp new file mode 100644 index 0000000000..d5a910e41c --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/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/ScControlMathTest.cpp b/src/modules/spacecraft/SpacecraftPositionControl/ScControlMathTest.cpp new file mode 100644 index 0000000000..e9b6fba12f --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/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/ScPositionControlTest.cpp b/src/modules/spacecraft/SpacecraftPositionControl/ScPositionControlTest.cpp new file mode 100644 index 0000000000..a9ce422ed5 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftPositionControl/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/SpacecraftRateControl/SpacecraftRateControl.cpp b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp new file mode 100644 index 0000000000..c92b9ad80a --- /dev/null +++ b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp @@ -0,0 +1,281 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 "SpacecraftRateControl.hpp" + +#include +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +SpacecraftRateControl::SpacecraftRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _controller_status_pub.advertise(); + updateParams(); +} + +void SpacecraftRateControl::updateParams() +{ + ModuleParams::updateParams(); + // rate control parameters + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + const Vector3f rate_k = Vector3f(_param_sc_rollrate_k.get(), _param_sc_pitchrate_k.get(), _param_sc_yawrate_k.get()); + + _rate_control.setPidGains( + rate_k.emult(Vector3f(_param_sc_rollrate_p.get(), _param_sc_pitchrate_p.get(), _param_sc_yawrate_p.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_i.get(), _param_sc_pitchrate_i.get(), _param_sc_yawrate_i.get())), + rate_k.emult(Vector3f(_param_sc_rollrate_d.get(), _param_sc_pitchrate_d.get(), _param_sc_yawrate_d.get()))); + + _rate_control.setIntegratorLimit( + Vector3f(_param_sc_rr_int_lim.get(), _param_sc_pr_int_lim.get(), _param_sc_yr_int_lim.get())); + + _rate_control.setFeedForwardGain( + Vector3f(_param_sc_rollrate_ff.get(), _param_sc_pitchrate_ff.get(), _param_sc_yawrate_ff.get())); + + // manual rate control acro mode rate limits + _acro_rate_max = Vector3f(radians(_param_sc_acro_r_max.get()), radians(_param_sc_acro_p_max.get()), + radians(_param_sc_acro_y_max.get())); + _manual_force_max = _param_sc_manual_f_max.get(); + _manual_torque_max = _param_sc_manual_t_max.get(); +} + +void SpacecraftRateControl::updateRateControl() +{ + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) + { + const hrt_abstime now = angular_velocity.timestamp_sample; + + // Guard against too small (< 0.125ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f); + _last_run = now; + + const Vector3f rates{angular_velocity.xyz}; + const Vector3f angular_accel{angular_velocity.xyz_derivative}; + + /* check for updates in other topics */ + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; + } + } + + _vehicle_status_sub.update(&_vehicle_status); + + // use rates setpoint topic + vehicle_rates_setpoint_s vehicle_rates_setpoint{}; + + if (_vehicle_control_mode.flag_control_manual_enabled && + !_vehicle_control_mode.flag_control_attitude_enabled) { + // Here we can be in: Manual Mode or Acro Mode + // generate the rate setpoint from sticks + manual_control_setpoint_s manual_control_setpoint; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + if (_vehicle_control_mode.flag_control_rates_enabled) { + // manual rates control - ACRO mode + const Vector3f man_rate_sp{manual_control_setpoint.roll, + -manual_control_setpoint.pitch, + manual_control_setpoint.yaw}; + + _rates_setpoint = man_rate_sp * 5; + _thrust_setpoint(2) = -manual_control_setpoint.throttle; + _thrust_setpoint(0) = _thrust_setpoint(1) = 0.f; + + // publish rate setpoint + vehicle_rates_setpoint.roll = _rates_setpoint(0); + vehicle_rates_setpoint.pitch = _rates_setpoint(1); + vehicle_rates_setpoint.yaw = _rates_setpoint(2); + _thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body); + vehicle_rates_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint); + + } else if (!_vehicle_control_mode.flag_control_rates_enabled) { + // Manual/direct control + // Yaw stick commands rotational moment, Roll/Pitch stick commands translational forces + // All other axis are set as zero (We only have four channels on the manual control inputs) + _thrust_setpoint(0) = math::constrain((manual_control_setpoint.pitch * _manual_force_max), -1.f, 1.f); + _thrust_setpoint(1) = math::constrain((manual_control_setpoint.roll * _manual_force_max), -1.f, 1.f); + _thrust_setpoint(2) = 0.0; + + _torque_setpoint(0) = _torque_setpoint(1) = 0.0; + _torque_setpoint(2) = math::constrain((manual_control_setpoint.yaw * _manual_torque_max), -1.f, 1.f); + + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + _torque_setpoint.copyTo(vehicle_torque_setpoint.xyz); + + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); + + updateActuatorControlsStatus(vehicle_torque_setpoint, dt); + } + } + + } else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) { + // Get rates from other controllers (e.g. position or attitude controller) + if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) { + _rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0); + _rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1); + _rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2); + _thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body); + } + } + + // run the rate controller + if (_vehicle_control_mode.flag_control_rates_enabled) { + // reset integral if disarmed + if (!_vehicle_control_mode.flag_armed || + _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_SPACECRAFT) { + _rate_control.resetIntegral(); + } + + // update saturation status from control allocation feedback + control_allocator_status_s control_allocator_status; + + if (_control_allocator_status_sub.update(&control_allocator_status)) { + Vector saturation_positive; + Vector saturation_negative; + + if (!control_allocator_status.torque_setpoint_achieved) { + for (size_t i = 0; i < 3; i++) { + if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { + saturation_positive(i) = true; + + } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { + saturation_negative(i) = true; + } + } + } + + // TODO: send the unallocated value directly for better anti-windup + // _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + } + + // run rate controller + const Vector3f torque_sp = + _rate_control.update(rates, _rates_setpoint, angular_accel, dt, false); + // PX4_INFO("Rate: %f %f %f", (double)rates(0), (double)rates(1), (double)rates(2)); + // PX4_INFO("Rate Setpoint: %f %f %f", (double)_rates_setpoint(0), (double)_rates_setpoint(1), (double)_rates_setpoint(2)); + // PX4_INFO("Torque sp: %f %f %f", (double)torque_sp(0), (double)torque_sp(1), (double)torque_sp(2)); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); + + // publish thrust and torque setpoints + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + + _thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz); + vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(torque_sp(0)) ? torque_sp(0) : 0.f; + vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(torque_sp(1)) ? torque_sp(1) : 0.f; + vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(torque_sp(2)) ? torque_sp(2) : 0.f; + + // scale setpoints by battery status if enabled + if (_param_sc_bat_scale_en.get()) { + if (_battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) { + _battery_status_scale = battery_status.scale; + } + } + + if (_battery_status_scale > 0.f) { + for (int i = 0; i < 3; i++) { + vehicle_thrust_setpoint.xyz[i] = + math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + vehicle_torque_setpoint.xyz[i] = + math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f); + } + } + } + + vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_thrust_setpoint.timestamp = hrt_absolute_time(); + _vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint); + + vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample; + vehicle_torque_setpoint.timestamp = hrt_absolute_time(); + _vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint); + + updateActuatorControlsStatus(vehicle_torque_setpoint, dt); + } + } +} + +void SpacecraftRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, + float dt) +{ + for (int i = 0; i < 3; i++) { + _control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt; + } + + _energy_integration_time += dt; + + if (_energy_integration_time > 500e-3f) { + actuator_controls_status_s status; + status.timestamp = vehicle_torque_setpoint.timestamp; + + for (int i = 0; i < 3; i++) { + status.control_power[i] = _control_energy[i] / _energy_integration_time; + _control_energy[i] = 0.f; + } + + _actuator_controls_status_pub.publish(status); + _energy_integration_time = 0.f; + } +} diff --git a/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp new file mode 100644 index 0000000000..f239d5f866 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +#pragma once + +#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 SpacecraftRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + SpacecraftRateControl(ModuleParams *parent); + ~SpacecraftRateControl() = default; + + /** @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; + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt); + + RateControl _rate_control; ///< class for rate control calculations + + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::Publication _actuator_controls_status_pub{ORB_ID(actuator_controls_status_0)}; + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)}; + uORB::Publication _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)}; + + vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_status_s _vehicle_status{}; + + bool _landed{true}; + bool _maybe_landed{true}; + + hrt_abstime _last_run{0}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + // keep setpoint values between updates + matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ + matrix::Vector3f _rates_setpoint{}; + float _manual_torque_max{1.0}; + float _manual_force_max{1.0}; + + float _battery_status_scale{0.0f}; + matrix::Vector3f _thrust_setpoint{}; + matrix::Vector3f _torque_setpoint{}; + + float _energy_integration_time{0.0f}; + float _control_energy[4] {}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sc_rollrate_p, + (ParamFloat) _param_sc_rollrate_i, + (ParamFloat) _param_sc_rr_int_lim, + (ParamFloat) _param_sc_rollrate_d, + (ParamFloat) _param_sc_rollrate_ff, + (ParamFloat) _param_sc_rollrate_k, + + (ParamFloat) _param_sc_pitchrate_p, + (ParamFloat) _param_sc_pitchrate_i, + (ParamFloat) _param_sc_pr_int_lim, + (ParamFloat) _param_sc_pitchrate_d, + (ParamFloat) _param_sc_pitchrate_ff, + (ParamFloat) _param_sc_pitchrate_k, + + (ParamFloat) _param_sc_yawrate_p, + (ParamFloat) _param_sc_yawrate_i, + (ParamFloat) _param_sc_yr_int_lim, + (ParamFloat) _param_sc_yawrate_d, + (ParamFloat) _param_sc_yawrate_ff, + (ParamFloat) _param_sc_yawrate_k, + + (ParamFloat) _param_sc_acro_r_max, + (ParamFloat) _param_sc_acro_p_max, + (ParamFloat) _param_sc_acro_y_max, + (ParamFloat) _param_sc_acro_expo, /**< expo stick curve shape (roll & pitch) */ + (ParamFloat) _param_sc_acro_expo_y, /**< expo stick curve shape (yaw) */ + (ParamFloat) _param_sc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ + (ParamFloat) _param_sc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ + + (ParamFloat) _param_sc_manual_f_max, + (ParamFloat) _param_sc_manual_t_max, + + (ParamBool) _param_sc_bat_scale_en + ) +}; diff --git a/src/modules/spacecraft/SpacecraftRateControl/sc_rate_control_params.c b/src/modules/spacecraft/SpacecraftRateControl/sc_rate_control_params.c new file mode 100644 index 0000000000..f3b194f3b7 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftRateControl/sc_rate_control_params.c @@ -0,0 +1,422 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 mc_rate_control_params.c + * Parameters for multicopter attitude controller. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.01 + * @max 0.5 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_P, 0.15f); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_I, 0.2f); + +/** + * Roll rate integrator limit + * + * Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_RR_INT_LIM, 0.30f); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @max 0.01 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_D, 0.003f); + +/** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_FF, 0.0f); + +/** + * Roll rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = SC_ROLLRATE_K * (SC_ROLLRATE_P * error + * + SC_ROLLRATE_I * error_integral + * + SC_ROLLRATE_D * error_derivative) + * Set SC_ROLLRATE_P=1 to implement a PID in the ideal form. + * Set SC_ROLLRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.01 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_K, 1.0f); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.01 + * @max 0.6 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_P, 0.15f); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_I, 0.2f); + +/** + * Pitch rate integrator limit + * + * Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PR_INT_LIM, 0.30f); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_D, 0.003f); + +/** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_FF, 0.0f); + +/** + * Pitch rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = SC_PITCHRATE_K * (SC_PITCHRATE_P * error + * + SC_PITCHRATE_I * error_integral + * + SC_PITCHRATE_D * error_derivative) + * Set SC_PITCHRATE_P=1 to implement a PID in the ideal form. + * Set SC_PITCHRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.01 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_K, 1.0f); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @max 10.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_P, 10.0f); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_I, 0.865f); + +/** + * Yaw rate integrator limit + * + * Yaw rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YR_INT_LIM, 0.2f); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_D, 0.0f); + +/** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_FF, 0.0f); + +/** + * Yaw rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = SC_YAWRATE_K * (SC_YAWRATE_P * error + * + SC_YAWRATE_I * error_integral + * + SC_YAWRATE_D * error_derivative) + * Set SC_YAWRATE_P=1 to implement a PID in the ideal form. + * Set SC_YAWRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_K, 1.0f); + +/** + * Max acro roll rate + * + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_R_MAX, 720.0f); + +/** + * Max acro pitch rate + * + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_P_MAX, 720.0f); + +/** + * Max acro yaw rate + * + * default 1.5 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_Y_MAX, 540.0f); + +/** + * Acro mode Expo factor for Roll and Pitch. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_EXPO, 0.69f); + +/** + * Acro mode Expo factor for Yaw. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_EXPO_Y, 0.69f); + +/** + * Acro mode SuperExpo factor for Roll and Pitch. + * + * SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPO, 0.7f); + +/** + * Acro mode SuperExpo factor for Yaw. + * + * SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO_Y. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPOY, 0.7f); + +/** + * Battery power level scaler + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The copter + * should constantly behave as if it was fully charged with reduced max acceleration + * at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group Multicopter Rate Control + */ +PARAM_DEFINE_INT32(SC_BAT_SCALE_EN, 0); + +/** + * Manual mode maximum force. + * + * * + * @min 0 + * @max 1.0 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_F_MAX, 1.0f); + +/** + * Manual mode maximum torque. + * + * * + * @min 0 + * @max 1.0 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_T_MAX, 1.0f);