diff --git a/src/modules/spacecraft/CMakeLists.txt b/src/modules/spacecraft/CMakeLists.txt index 330385c9f0..a4a3c6ded4 100644 --- a/src/modules/spacecraft/CMakeLists.txt +++ b/src/modules/spacecraft/CMakeLists.txt @@ -33,6 +33,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(SpacecraftRateControl) +add_subdirectory(SpacecraftAttitudeControl) px4_add_module( MODULE modules__spacecraft @@ -50,4 +51,5 @@ px4_add_module( mathlib px4_work_queue SpacecraftRateControl + SpacecraftAttitudeControl ) diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.cpp new file mode 100644 index 0000000000..7c335485ea --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/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/AttitudeControl.hpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControl.hpp new file mode 100644 index 0000000000..3d702055b8 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/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/AttitudeControl/AttitudeControlMath.hpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/AttitudeControlMath.hpp new file mode 100644 index 0000000000..fe0f760ed4 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/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/AttitudeControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/CMakeLists.txt new file mode 100644 index 0000000000..825f96d2f9 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/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(AttitudeControlLibrary + AttitudeControl.cpp + AttitudeControl.hpp + AttitudeControlMath.hpp +) +target_compile_options(AttitudeControlLibrary PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(AttitudeControlLibrary 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/AttitudeControl/ScAttitudeControlMathTest.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlMathTest.cpp new file mode 100644 index 0000000000..b247fce074 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/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/AttitudeControl/ScAttitudeControlTest.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/ScAttitudeControlTest.cpp new file mode 100644 index 0000000000..c7c20c5a62 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl/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/SpacecraftAttitudeControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt new file mode 100644 index 0000000000..0985fe3f19 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015-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. +# +############################################################################ + +add_subdirectory(AttitudeControl) + +px4_add_library(SpacecraftAttitudeControl +SpacecraftAttitudeControl.cpp +) + +target_link_libraries(SpacecraftAttitudeControl PUBLIC mathlib) +target_link_libraries(SpacecraftAttitudeControl PUBLIC AttitudeControlLibrary) +target_include_directories(SpacecraftAttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.cpp new file mode 100644 index 0000000000..5b749f965a --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.cpp @@ -0,0 +1,238 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2018 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 sc_att_control_main.cpp + * Spacecraft attitude controller. + * Based off the multicopter attitude controller module. + * + * @author Pedro Roque, + * + */ + +#include "SpacecraftAttitudeControl.hpp" + +#include +#include +#include + +#include "AttitudeControl/AttitudeControlMath.hpp" + +using namespace matrix; + +SpacecraftAttitudeControl::SpacecraftAttitudeControl(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); +} + +void +SpacecraftAttitudeControl::updateParams() +{ + ModuleParams::updateParams(); + // Store some of the parameters in a more convenient way & precompute often-used values + _attitude_control.setProportionalGain(Vector3f(_param_sc_roll_p.get(), _param_sc_pitch_p.get(), _param_sc_yaw_p.get())); + + // angular rate limits + using math::radians; + _attitude_control.setRateLimit(Vector3f(radians(_param_sc_rollrate_max.get()), radians(_param_sc_pitchrate_max.get()), + radians(_param_sc_yawrate_max.get()))); + _man_tilt_max = math::radians(_param_sc_man_tilt_max.get()); +} + +void +SpacecraftAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, bool reset_yaw_sp) +{ + vehicle_attitude_setpoint_s attitude_setpoint{}; + const float yaw = Eulerf(q).psi(); + + attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.yaw * math::radians(_param_sc_man_y_scale.get()); + + // Avoid accumulating absolute yaw error with arming stick gesture in case heading_good_for_control stays true + if (_manual_control_setpoint.throttle < -.9f) { + reset_yaw_sp = true; + } + + // Make sure not absolute heading error builds up + if (reset_yaw_sp) { + _man_yaw_sp = yaw; + + } else { + _man_yaw_sp = wrap_pi(_man_yaw_sp + attitude_setpoint.yaw_sp_move_rate * dt); + } + + /* + * Input mapping for roll & pitch setpoints + * ---------------------------------------- + * We control the following 2 angles: + * - tilt angle, given by sqrt(roll*roll + pitch*pitch) + * - the direction of the maximum tilt in the XY-plane, which also defines the direction of the motion + * + * This allows a simple limitation of the tilt angle, the vehicle flies towards the direction that the stick + * points to, and changes of the stick input are linear. + */ + _man_roll_input_filter.setParameters(dt, _param_sc_man_tilt_tau.get()); + _man_pitch_input_filter.setParameters(dt, _param_sc_man_tilt_tau.get()); + + Vector2f v = Vector2f(_man_roll_input_filter.update(_manual_control_setpoint.roll * _man_tilt_max), + -_man_pitch_input_filter.update(_manual_control_setpoint.pitch * _man_tilt_max)); + float v_norm = v.norm(); // the norm of v defines the tilt angle + + if (v_norm > _man_tilt_max) { // limit to the configured maximum tilt angle + v *= _man_tilt_max / v_norm; + } + + Quatf q_sp_rp = AxisAnglef(v(0), v(1), 0.f); + // The axis angle can change the yaw as well (noticeable at higher tilt angles). + // This is the formula by how much the yaw changes: + // let a := tilt angle, b := atan(y/x) (direction of maximum tilt) + // yaw = atan(-2 * sin(b) * cos(b) * sin^2(a/2) / (1 - 2 * cos^2(b) * sin^2(a/2))). + const Quatf q_sp_yaw(cosf(_man_yaw_sp / 2.f), 0.f, 0.f, sinf(_man_yaw_sp / 2.f)); + + // Align the desired tilt with the yaw setpoint + Quatf q_sp = q_sp_yaw * q_sp_rp; + + q_sp.copyTo(attitude_setpoint.q_d); + + // Transform to euler angles for logging only + // const Eulerf euler_sp(q_sp); + // attitude_setpoint.roll_body = euler_sp(0); + // attitude_setpoint.pitch_body = euler_sp(1); + // attitude_setpoint.yaw_body = euler_sp(2); + // attitude_setpoint.q_d[0] = q_sp; + + attitude_setpoint.timestamp = hrt_absolute_time(); + + _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + + // update attitude controller setpoint immediately + _attitude_control.setAttitudeSetpoint(q_sp); + _thrust_setpoint_body = Vector3f(attitude_setpoint.thrust_body); + _last_attitude_setpoint = attitude_setpoint.timestamp; +} + +void +SpacecraftAttitudeControl::updateAttitudeControl() +{ + // run controller on attitude updates + vehicle_attitude_s v_att; + + if (_vehicle_attitude_sub.update(&v_att)) { + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((v_att.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f); + _last_run = v_att.timestamp_sample; + + const Quatf q{v_att.q}; + + // Check for new attitude setpoint + if (_vehicle_attitude_setpoint_sub.updated()) { + vehicle_attitude_setpoint_s vehicle_attitude_setpoint; + + if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint) + && (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) { + _attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d)); + _thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body); + _last_attitude_setpoint = vehicle_attitude_setpoint.timestamp; + } + } + + // Check for a heading reset + if (_quat_reset_counter != v_att.quat_reset_counter) { + const Quatf delta_q_reset(v_att.delta_q_reset); + + // for stabilized attitude generation only extract the heading change from the delta quaternion + _man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi()); + + if (v_att.timestamp > _last_attitude_setpoint) { + // adapt existing attitude setpoint unless it was generated after the current attitude estimate + _attitude_control.adaptAttitudeSetpoint(delta_q_reset); + } + + _quat_reset_counter = v_att.quat_reset_counter; + } + + /* check for updates in other topics */ + _manual_control_setpoint_sub.update(&_manual_control_setpoint); + _vehicle_control_mode_sub.update(&_vehicle_control_mode); + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _vehicle_type_spacecraft = (vehicle_status.system_type == vehicle_status_s::VEHICLE_TYPE_SPACECRAFT); + + } + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position; + + if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { + _heading_good_for_control = vehicle_local_position.heading_good_for_control; + } + } + + bool attitude_setpoint_generated = false; + + if (_vehicle_control_mode.flag_control_attitude_enabled) { + + // Generate the attitude setpoint from stick inputs if we are in Stabilized mode + if (_vehicle_control_mode.flag_control_manual_enabled && + !_vehicle_control_mode.flag_control_altitude_enabled && + !_vehicle_control_mode.flag_control_velocity_enabled && + !_vehicle_control_mode.flag_control_position_enabled) { + generate_attitude_setpoint(q, dt, _reset_yaw_sp); + attitude_setpoint_generated = true; + + } else { + _man_roll_input_filter.reset(0.f); + _man_pitch_input_filter.reset(0.f); + } + + Vector3f rates_sp = _attitude_control.update(q); + + // publish rate setpoint + vehicle_rates_setpoint_s rates_setpoint{}; + rates_setpoint.roll = rates_sp(0); + rates_setpoint.pitch = rates_sp(1); + rates_setpoint.yaw = rates_sp(2); + _thrust_setpoint_body.copyTo(rates_setpoint.thrust_body); + rates_setpoint.timestamp = hrt_absolute_time(); + _vehicle_rates_setpoint_pub.publish(rates_setpoint); + } + + // reset yaw setpoint during transitions, tailsitter.cpp generates + // attitude setpoint for the transition + _reset_yaw_sp = !attitude_setpoint_generated || !_heading_good_for_control; + } +} diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp b/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp new file mode 100644 index 0000000000..5358fc7fcf --- /dev/null +++ b/src/modules/spacecraft/SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * 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 + +class SpacecraftAttitudeControl : public ModuleParams +{ +public: + SpacecraftAttitudeControl(ModuleParams *parent); + ~SpacecraftAttitudeControl() = default; + + void updateAttitudeControl(); + +protected: + + /** + * initialize some vectors/matrices from parameters + */ + void updateParams(); + +private: + + /** + * Generate & publish an attitude setpoint from stick inputs + */ + void generate_attitude_setpoint(const matrix::Quatf &q, float dt, bool reset_yaw_sp); + + ScAttitudeControl _attitude_control; /**< class for attitude control calculations */ + + // Attitude setpoint and current attitude sub + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + + // Manual control setpoint sub + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + + // Vehicle control mode sub and status + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + // Vehicle local position sub + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + + // Publish rate setpoint for rate controller and att_control status + uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::Publication _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)}; /**< attitude setpoint publication */ + + manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */ + vehicle_control_mode_s _vehicle_control_mode {}; /**< vehicle control mode */ + + matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + + AlphaFilter _man_roll_input_filter; + AlphaFilter _man_pitch_input_filter; + + hrt_abstime _last_run{0}; + hrt_abstime _last_attitude_setpoint{0}; + + bool _reset_yaw_sp{true}; + bool _heading_good_for_control{true}; ///< initialized true to have heading lock when local position never published + bool _vehicle_type_spacecraft{true}; + + uint8_t _quat_reset_counter{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_sc_man_tilt_tau, + + (ParamFloat) _param_sc_roll_p, + (ParamFloat) _param_sc_pitch_p, + (ParamFloat) _param_sc_yaw_p, + (ParamFloat) _param_sc_yaw_weight, + + (ParamFloat) _param_sc_rollrate_max, + (ParamFloat) _param_sc_pitchrate_max, + (ParamFloat) _param_sc_yawrate_max, + (ParamFloat) _param_sc_man_tilt_max, + + /* Stabilized mode params */ + (ParamFloat) _param_sc_man_y_scale /**< scaling factor from stick to yaw rate */ + ) +}; + diff --git a/src/modules/spacecraft/SpacecraftHandler.cpp b/src/modules/spacecraft/SpacecraftHandler.cpp index 935380ba68..175ad8f8ca 100644 --- a/src/modules/spacecraft/SpacecraftHandler.cpp +++ b/src/modules/spacecraft/SpacecraftHandler.cpp @@ -72,6 +72,7 @@ void SpacecraftHandler::Run() _timestamp = hrt_absolute_time(); _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + _spacecraft_attitude_control.updateAttitudeControl(); _spacecraft_rate_control.updateRateControl(); // TODO: Prepare allocation diff --git a/src/modules/spacecraft/SpacecraftHandler.hpp b/src/modules/spacecraft/SpacecraftHandler.hpp index 7d445a1931..c9b9e293e8 100644 --- a/src/modules/spacecraft/SpacecraftHandler.hpp +++ b/src/modules/spacecraft/SpacecraftHandler.hpp @@ -64,6 +64,7 @@ // Local includes #include "SpacecraftRateControl/SpacecraftRateControl.hpp" +#include "SpacecraftAttitudeControl/SpacecraftAttitudeControl.hpp" class SpacecraftHandler : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { @@ -107,6 +108,7 @@ private: // Class instances SpacecraftRateControl _spacecraft_rate_control{this}; + SpacecraftAttitudeControl _spacecraft_attitude_control{this}; // Variables hrt_abstime _timestamp{0}; diff --git a/src/modules/spacecraft/spacecraft_attitude_params.c b/src/modules/spacecraft/spacecraft_attitude_params.c new file mode 100644 index 0000000000..6e2c27b192 --- /dev/null +++ b/src/modules/spacecraft/spacecraft_attitude_params.c @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 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 sc_att_control_params.c + * Parameters for spacecraft attitude controller. + * + * @author Pedro Roque, + */ + +/** + * Roll P gain + * + * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 12 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_ROLL_P, 6.5f); + +/** + * Pitch P gain + * + * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 12 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_PITCH_P, 6.5f); + +/** + * Yaw P gain + * + * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @max 5 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_YAW_P, 2.8f); + +/** + * Yaw weight + * + * A fraction [0,1] deprioritizing yaw compared to roll and pitch in non-linear attitude control. + * Deprioritizing yaw is necessary because multicopters have much less control authority + * in yaw compared to the other axes and it makes sense because yaw is not critical for + * stable hovering or 3D navigation. + * + * For yaw control tuning use SC_YAW_P. This ratio has no impact on the yaw gain. + * + * @min 0.0 + * @max 1.0 + * @decimal 2 + * @increment 0.1 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_YAW_WEIGHT, 0.4f); + +/** + * Max roll rate + * + * Limit for roll rate in manual and auto modes (except acro). + * Has effect for large rotations in autonomous mode, to avoid large control + * output and mixer saturation. + * + * This is not only limited by the vehicle's properties, but also by the maximum + * measurement rate of the gyro. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_ROLLRATE_MAX, 220.0f); + +/** + * Max pitch rate + * + * Limit for pitch rate in manual and auto modes (except acro). + * Has effect for large rotations in autonomous mode, to avoid large control + * output and mixer saturation. + * + * This is not only limited by the vehicle's properties, but also by the maximum + * measurement rate of the gyro. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_PITCHRATE_MAX, 220.0f); + +/** + * Max yaw rate + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Spacecraft Attitude Control + */ +PARAM_DEFINE_FLOAT(SC_YAWRATE_MAX, 200.0f); + +/** + * Manual tilt input filter time constant + * + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0.0 + * @max 2.0 + * @decimal 2 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_TILT_TAU, 0.0f); + +/** + * Max manual yaw rate for Stabilized, Altitude, Position mode + * + * @unit deg/s + * @min 0 + * @max 400 + * @decimal 0 + * @increment 10 + * @group Spacecraft Position Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_Y_SCALE, 150.f); + +/** + * Maximal tilt angle in Stabilized or Manual mode + * + * @unit deg + * @min 0 + * @max 90 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(SC_MAN_TILT_MAX, 90.f); diff --git a/src/modules/spacecraft/spacecraft_params.c b/src/modules/spacecraft/spacecraft_rate_params.c similarity index 100% rename from src/modules/spacecraft/spacecraft_params.c rename to src/modules/spacecraft/spacecraft_rate_params.c