From de9755b33b07ee5ef08c7df0fb25df0fb10f7bab Mon Sep 17 00:00:00 2001 From: Pedro-Roque Date: Sun, 13 Apr 2025 14:51:38 +0200 Subject: [PATCH] feat: rate controller nominal --- .../airframes/71002_gz_spacecraft_2d | 2 +- .../init.d/airframes/70000_spacecraft_2d | 2 +- ROMFS/px4fmu_common/init.d/rc.sc_defaults | 4 +- ROMFS/px4fmu_common/init.d/rc.vehicle_setup | 9 + src/modules/spacecraft/CMakeLists.txt | 2 + .../AttitudeControl.cpp | 94 ----- .../AttitudeControl.hpp | 105 ----- .../AttitudeControlMath.hpp | 70 ---- .../SpacecraftAttitudeControl/CMakeLists.txt | 44 --- .../ScAttitudeControlMathTest.cpp | 92 ----- .../ScAttitudeControlTest.cpp | 140 ------- src/modules/spacecraft/SpacecraftHandler.cpp | 25 ++ src/modules/spacecraft/SpacecraftHandler.hpp | 22 ++ .../SpacecraftPositionControl/ControlMath.cpp | 260 ------------- .../SpacecraftPositionControl/ControlMath.hpp | 118 ------ .../PositionControl.cpp | 220 ----------- .../PositionControl.hpp | 205 ---------- .../ScControlMathTest.cpp | 258 ------------ .../ScPositionControlTest.cpp | 368 ------------------ .../CMakeLists.txt | 17 +- .../SpacecraftRateControl.cpp | 23 +- .../SpacecraftRateControl.hpp | 32 +- src/modules/spacecraft/module.yaml | 5 +- ...e_control_params.c => spacecraft_params.c} | 65 ++-- 24 files changed, 117 insertions(+), 2065 deletions(-) delete mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.cpp delete mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.hpp delete mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControlMath.hpp delete mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt delete mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlMathTest.cpp delete mode 100644 src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlTest.cpp delete mode 100644 src/modules/spacecraft/SpacecraftPositionControl/ControlMath.cpp delete mode 100644 src/modules/spacecraft/SpacecraftPositionControl/ControlMath.hpp delete mode 100644 src/modules/spacecraft/SpacecraftPositionControl/PositionControl.cpp delete mode 100644 src/modules/spacecraft/SpacecraftPositionControl/PositionControl.hpp delete mode 100644 src/modules/spacecraft/SpacecraftPositionControl/ScControlMathTest.cpp delete mode 100644 src/modules/spacecraft/SpacecraftPositionControl/ScPositionControlTest.cpp rename src/modules/spacecraft/{SpacecraftPositionControl => SpacecraftRateControl}/CMakeLists.txt (77%) rename src/modules/spacecraft/{SpacecraftRateControl/sc_rate_control_params.c => spacecraft_params.c} (88%) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d index 4cc0a02e41..4396d9add0 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d @@ -20,7 +20,7 @@ param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later? param set-default CA_AIRFRAME 14 -param set-default MAV_TYPE 99 +param set-default MAV_TYPE 7 # Using Airship param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d b/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d index c0f55737f5..4bf69a47f6 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d @@ -11,7 +11,7 @@ . ${R}etc/init.d/rc.sc_defaults param set-default CA_AIRFRAME 14 -param set-default MAV_TYPE 99 +param set-default MAV_TYPE 7 param set-default CA_THRUSTER_CNT 8 param set-default CA_R_REV 0 diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults index 8dc8867b22..5892ea62e5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.sc_defaults @@ -5,8 +5,8 @@ set VEHICLE_TYPE sc -# MAV_TYPE_QUADROTOR 2 -#param set-default MAV_TYPE 12 +# MAV_TYPE_SPACECRAFT +param set-default MAV_TYPE 7 # Set micro-dds-client to use ethernet and IP-address 192.168.0.1 param set-default UXRCE_DDS_AG_IP -1062731775 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index ae34645ad8..b0557a31e5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -32,6 +32,15 @@ then . ${R}etc/init.d/rc.rover_apps fi +# +# Spapcecraft setup. +# +if [ $VEHICLE_TYPE = sc ] +then + # Start standard multicopter apps. + . ${R}etc/init.d/rc.sc_apps +fi + # # Differential Rover setup. # diff --git a/src/modules/spacecraft/CMakeLists.txt b/src/modules/spacecraft/CMakeLists.txt index 88a18e9c01..330385c9f0 100644 --- a/src/modules/spacecraft/CMakeLists.txt +++ b/src/modules/spacecraft/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) +add_subdirectory(SpacecraftRateControl) px4_add_module( MODULE modules__spacecraft @@ -48,4 +49,5 @@ px4_add_module( DEPENDS mathlib px4_work_queue + SpacecraftRateControl ) diff --git a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.cpp b/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.cpp deleted file mode 100644 index 7c335485ea..0000000000 --- a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.cpp +++ /dev/null @@ -1,94 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index 3d702055b8..0000000000 --- a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.hpp +++ /dev/null @@ -1,105 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index fe0f760ed4..0000000000 --- a/src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControlMath.hpp +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index ab68cd5a74..0000000000 --- a/src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# 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 deleted file mode 100644 index b247fce074..0000000000 --- a/src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlMathTest.cpp +++ /dev/null @@ -1,92 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index c7c20c5a62..0000000000 --- a/src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlTest.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/**************************************************************************** - * - * 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 c397dbd3f6..935380ba68 100644 --- a/src/modules/spacecraft/SpacecraftHandler.cpp +++ b/src/modules/spacecraft/SpacecraftHandler.cpp @@ -57,6 +57,31 @@ bool SpacecraftHandler::init() return true; } +void SpacecraftHandler::updateParams() +{ + ModuleParams::updateParams(); +} + +void SpacecraftHandler::Run() +{ + if (_parameter_update_sub.updated()) { + updateParams(); + } + + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + _spacecraft_rate_control.updateRateControl(); + + // TODO: Prepare allocation + // if (_vehicle_control_mode.flag_armed) { + // generateActuatorSetpoint(); + + // } + +} + int SpacecraftHandler::task_spawn(int argc, char *argv[]) { SpacecraftHandler *instance = new SpacecraftHandler(); diff --git a/src/modules/spacecraft/SpacecraftHandler.hpp b/src/modules/spacecraft/SpacecraftHandler.hpp index 28380d2ca4..7d445a1931 100644 --- a/src/modules/spacecraft/SpacecraftHandler.hpp +++ b/src/modules/spacecraft/SpacecraftHandler.hpp @@ -62,6 +62,9 @@ #include #include +// Local includes +#include "SpacecraftRateControl/SpacecraftRateControl.hpp" + class SpacecraftHandler : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -90,4 +93,23 @@ protected: */ void updateParams() override; +private: + void Run() override; + + // uORB subscriptions + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + + // Class instances + SpacecraftRateControl _spacecraft_rate_control{this}; + + // Variables + hrt_abstime _timestamp{0}; + float _dt{0.f}; + }; diff --git a/src/modules/spacecraft/SpacecraftPositionControl/ControlMath.cpp b/src/modules/spacecraft/SpacecraftPositionControl/ControlMath.cpp deleted file mode 100644 index d2228d5597..0000000000 --- a/src/modules/spacecraft/SpacecraftPositionControl/ControlMath.cpp +++ /dev/null @@ -1,260 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index c2d9b8b7f8..0000000000 --- a/src/modules/spacecraft/SpacecraftPositionControl/ControlMath.hpp +++ /dev/null @@ -1,118 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index 6d9ea12767..0000000000 --- a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl.cpp +++ /dev/null @@ -1,220 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index d5a910e41c..0000000000 --- a/src/modules/spacecraft/SpacecraftPositionControl/PositionControl.hpp +++ /dev/null @@ -1,205 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index e9b6fba12f..0000000000 --- a/src/modules/spacecraft/SpacecraftPositionControl/ScControlMathTest.cpp +++ /dev/null @@ -1,258 +0,0 @@ -/**************************************************************************** - * - * 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 deleted file mode 100644 index a9ce422ed5..0000000000 --- a/src/modules/spacecraft/SpacecraftPositionControl/ScPositionControlTest.cpp +++ /dev/null @@ -1,368 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include - -using namespace matrix; - -TEST(PositionControlTest, EmptySetpoint) -{ - ScPositionControl position_control; - - vehicle_attitude_setpoint_s attitude{}; - position_control.getAttitudeSetpoint(attitude); - EXPECT_FLOAT_EQ(attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f); - EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f); - EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); - EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); - EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); - EXPECT_EQ(attitude.reset_integral, false); - EXPECT_EQ(attitude.fw_control_yaw_wheel, false); -} - -class PositionControlBasicTest : public ::testing::Test -{ -public: - PositionControlBasicTest() - { - _position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f)); - _position_control.setVelocityGains(Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f), Vector3f(20.f, 20.f, 20.f)); - _position_control.setVelocityLimits(1.f, 1.f, 1.f); - _position_control.setThrustLimits(0.1f, MAXIMUM_THRUST); - _position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN); - _position_control.setTiltLimit(1.f); - _position_control.setHoverThrust(.5f); - } - - bool runController() - { - _position_control.setInputSetpoint(_input_setpoint); - const bool ret = _position_control.update(.1f); - _position_control.getAttitudeSetpoint(_attitude); - return ret; - } - - ScPositionControl _position_control; - trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint}; - vehicle_local_position_setpoint_s _output_setpoint{}; - vehicle_attitude_setpoint_s _attitude{}; - - static constexpr float MAXIMUM_THRUST = 0.9f; - static constexpr float HORIZONTAL_THRUST_MARGIN = 0.3f; -}; - -class PositionControlBasicDirectionTest : public PositionControlBasicTest -{ -public: - void checkDirection() - { - Vector3f thrust(_output_setpoint.thrust); - EXPECT_GT(thrust(0), 0.f); - EXPECT_GT(thrust(1), 0.f); - EXPECT_LT(thrust(2), 0.f); - - Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); - EXPECT_LT(body_z(0), 0.f); - EXPECT_LT(body_z(1), 0.f); - EXPECT_GT(body_z(2), 0.f); - } -}; - -TEST_F(PositionControlBasicDirectionTest, PositionDirection) -{ - Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.position); - EXPECT_TRUE(runController()); - checkDirection(); -} - -TEST_F(PositionControlBasicDirectionTest, VelocityDirection) -{ - Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.velocity); - EXPECT_TRUE(runController()); - checkDirection(); -} - -TEST_F(PositionControlBasicTest, TiltLimit) -{ - Vector3f(10.f, 10.f, 0.f).copyTo(_input_setpoint.position); - - EXPECT_TRUE(runController()); - Vector3f body_z = Quatf(_attitude.q_d).dcm_z(); - float angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); - EXPECT_GT(angle, 0.f); - EXPECT_LE(angle, 1.f); - - _position_control.setTiltLimit(0.5f); - EXPECT_TRUE(runController()); - body_z = Quatf(_attitude.q_d).dcm_z(); - angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f))); - EXPECT_GT(angle, 0.f); - EXPECT_LE(angle, .50001f); - - _position_control.setTiltLimit(1.f); // restore original -} - -TEST_F(PositionControlBasicTest, VelocityLimit) -{ - Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); - - EXPECT_TRUE(runController()); - Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy); - EXPECT_LE(velocity_xy.norm(), 1.f); - EXPECT_LE(abs(_output_setpoint.vz), 1.f); -} - -TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) -{ - // Given a setpoint that drives the controller into vertical and horizontal saturation - Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position); - - // When you run it for one iteration - runController(); - Vector3f thrust(_output_setpoint.thrust); - - // Then the thrust vector length is limited by the maximum - EXPECT_FLOAT_EQ(thrust.norm(), MAXIMUM_THRUST); - - // Then the horizontal thrust is limited by its margin - EXPECT_FLOAT_EQ(thrust(0), HORIZONTAL_THRUST_MARGIN / sqrt(2.f)); - EXPECT_FLOAT_EQ(thrust(1), HORIZONTAL_THRUST_MARGIN / sqrt(2.f)); - EXPECT_FLOAT_EQ(thrust(2), - -sqrt(MAXIMUM_THRUST * MAXIMUM_THRUST - HORIZONTAL_THRUST_MARGIN * HORIZONTAL_THRUST_MARGIN)); - thrust.print(); - - // Then the collective thrust is limited by the maximum - EXPECT_EQ(_attitude.thrust_body[0], 0.f); - EXPECT_EQ(_attitude.thrust_body[1], 0.f); - EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST); - - // Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust - EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); - // TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore - // EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); -} - -TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) -{ - Vector3f(10.f, 0.f, 10.f).copyTo(_input_setpoint.position); - - runController(); - Vector3f thrust(_output_setpoint.thrust); - EXPECT_FLOAT_EQ(thrust.length(), 0.1f); - - EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f); - - EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f); -} - -TEST_F(PositionControlBasicTest, FailsafeInput) -{ - _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f; - _input_setpoint.velocity[2] = .1f; - - EXPECT_TRUE(runController()); - EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f); - EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f); - EXPECT_LT(_output_setpoint.thrust[2], -.1f); - EXPECT_GT(_output_setpoint.thrust[2], -.5f); - EXPECT_GT(_attitude.thrust_body[2], -.5f); - EXPECT_LE(_attitude.thrust_body[2], -.1f); -} - -TEST_F(PositionControlBasicTest, IdleThrustInput) -{ - // High downwards acceleration to make sure there's no thrust - Vector3f(0.f, 0.f, 100.f).copyTo(_input_setpoint.acceleration); - - EXPECT_TRUE(runController()); - EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f); - EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f); - EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); // minimum thrust -} - -TEST_F(PositionControlBasicTest, InputCombinationsPosition) -{ - Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); - - EXPECT_TRUE(runController()); - EXPECT_FLOAT_EQ(_output_setpoint.x, .1f); - EXPECT_FLOAT_EQ(_output_setpoint.y, .2f); - EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); - EXPECT_FALSE(isnan(_output_setpoint.vx)); - EXPECT_FALSE(isnan(_output_setpoint.vy)); - EXPECT_FALSE(isnan(_output_setpoint.vz)); - EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); - EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); - EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); -} - -TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity) -{ - _input_setpoint.velocity[0] = .1f; - _input_setpoint.velocity[1] = .2f; - _input_setpoint.position[2] = .3f; // altitude - - EXPECT_TRUE(runController()); - EXPECT_TRUE(isnan(_output_setpoint.x)); - EXPECT_TRUE(isnan(_output_setpoint.y)); - EXPECT_FLOAT_EQ(_output_setpoint.z, .3f); - EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f); - EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f); - EXPECT_FALSE(isnan(_output_setpoint.vz)); - EXPECT_FALSE(isnan(_output_setpoint.thrust[0])); - EXPECT_FALSE(isnan(_output_setpoint.thrust[1])); - EXPECT_FALSE(isnan(_output_setpoint.thrust[2])); -} - -TEST_F(PositionControlBasicTest, SetpointValiditySimple) -{ - EXPECT_FALSE(runController()); - _input_setpoint.position[0] = .1f; - EXPECT_FALSE(runController()); - _input_setpoint.position[1] = .2f; - EXPECT_FALSE(runController()); - _input_setpoint.acceleration[2] = .3f; - EXPECT_TRUE(runController()); -} - -TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations) -{ - // This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly - float *const setpoint_loop_access_map[] = {&_input_setpoint.position[0], &_input_setpoint.velocity[0], &_input_setpoint.acceleration[0], - &_input_setpoint.position[1], &_input_setpoint.velocity[1], &_input_setpoint.acceleration[1], - &_input_setpoint.position[2], &_input_setpoint.velocity[2], &_input_setpoint.acceleration[2] - }; - - for (int combination = 0; combination < 512; combination++) { - _input_setpoint = PositionControl::empty_trajectory_setpoint; - - for (int j = 0; j < 9; j++) { - if (combination & (1 << j)) { - // Set arbitrary finite value, some values clearly hit the limits to check these corner case combinations - *(setpoint_loop_access_map[j]) = static_cast(combination) / static_cast(j + 1); - } - } - - // Expect at least one setpoint per axis - const bool has_x_setpoint = ((combination & 7) != 0); - const bool has_y_setpoint = (((combination >> 3) & 7) != 0); - const bool has_z_setpoint = (((combination >> 6) & 7) != 0); - // Expect xy setpoints to come in pairs - const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7); - const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs; - - EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl - << "input" << std::endl - << "position " << _input_setpoint.position[0] << ", " - << _input_setpoint.position[1] << ", " << _input_setpoint.position[2] << std::endl - << "velocity " << _input_setpoint.velocity[0] << ", " - << _input_setpoint.velocity[1] << ", " << _input_setpoint.velocity[2] << std::endl - << "acceleration " << _input_setpoint.acceleration[0] << ", " - << _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl - << "output" << std::endl - << "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl - << "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl - << "acceleration " << _output_setpoint.acceleration[0] << ", " - << _output_setpoint.acceleration[1] << ", " << _output_setpoint.acceleration[2] << std::endl; - } -} - -TEST_F(PositionControlBasicTest, InvalidState) -{ - Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position); - - PositionControlStates states{}; - states.position(0) = NAN; - _position_control.setState(states); - EXPECT_FALSE(runController()); - - states.velocity(0) = NAN; - _position_control.setState(states); - EXPECT_FALSE(runController()); - - states.position(0) = 0.f; - _position_control.setState(states); - EXPECT_FALSE(runController()); - - states.velocity(0) = 0.f; - states.acceleration(1) = NAN; - _position_control.setState(states); - EXPECT_FALSE(runController()); -} - - -TEST_F(PositionControlBasicTest, UpdateHoverThrust) -{ - // GIVEN: some hover thrust and 0 velocity change - const float hover_thrust = 0.6f; - _position_control.setHoverThrust(hover_thrust); - - Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); - - // WHEN: we run the controller - EXPECT_TRUE(runController()); - - // THEN: the output thrust equals the hover thrust - EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); - - // HOWEVER WHEN: we set a new hover thrust through the update function - const float hover_thrust_new = 0.7f; - _position_control.updateHoverThrust(hover_thrust_new); - EXPECT_TRUE(runController()); - - // THEN: the integral is updated to avoid discontinuities and - // the output is still the same - EXPECT_EQ(_output_setpoint.thrust[2], -hover_thrust); -} - -TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint) -{ - // GIVEN: the controller was ran with an invalid setpoint containing some valid values - _input_setpoint.position[0] = .1f; - _input_setpoint.position[1] = .2f; - // all z-axis setpoints stay NAN - EXPECT_FALSE(runController()); - - // WHEN: we run the controller with a valid setpoint - _input_setpoint = PositionControl::empty_trajectory_setpoint; - Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity); - EXPECT_TRUE(runController()); - - // THEN: the integral did not wind up and produce unexpected deviation - EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); -} diff --git a/src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt b/src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt similarity index 77% rename from src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt rename to src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt index 9b197ae5b1..b4f33ce25a 100644 --- a/src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt +++ b/src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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 @@ -31,14 +31,11 @@ # ############################################################################ -px4_add_library(SpacecraftPositionControl - ControlMath.cpp - ControlMath.hpp - PositionControl.cpp - PositionControl.hpp +px4_add_library(SpacecraftRateControl + SpacecraftRateControl.cpp ) -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) +target_link_libraries(SpacecraftRateControl PUBLIC RateControl) +target_link_libraries(SpacecraftRateControl PUBLIC mathlib) +target_link_libraries(SpacecraftRateControl PUBLIC circuit_breaker) +target_include_directories(SpacecraftRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp index c92b9ad80a..ca9bbcfa59 100644 --- a/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp +++ b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp @@ -90,16 +90,6 @@ void SpacecraftRateControl::updateRateControl() /* 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 @@ -175,8 +165,7 @@ void SpacecraftRateControl::updateRateControl() // 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) { + if (!_vehicle_control_mode.flag_armed) { _rate_control.resetIntegral(); } @@ -197,17 +186,9 @@ void SpacecraftRateControl::updateRateControl() } } } - - // 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)); + const Vector3f torque_sp = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, false); // publish rate controller status rate_ctrl_status_s rate_ctrl_status{}; diff --git a/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp index f239d5f866..cfb7a620cd 100644 --- a/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp +++ b/src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp @@ -62,31 +62,21 @@ using namespace time_literals; -class SpacecraftRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem +class SpacecraftRateControl : public ModuleParams { public: SpacecraftRateControl(ModuleParams *parent); ~SpacecraftRateControl() = default; - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); + /** + * @brief Update rate controller. + */ + void updateRateControl(); - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - bool init(); +protected: + void updateParams() override; 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 @@ -98,19 +88,19 @@ private: 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::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; 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{}; + vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_status_s _vehicle_status{}; + vehicle_angular_velocity_s angular_velocity{}; bool _landed{true}; bool _maybe_landed{true}; diff --git a/src/modules/spacecraft/module.yaml b/src/modules/spacecraft/module.yaml index 5b2b5a264d..1d6c2b576b 100644 --- a/src/modules/spacecraft/module.yaml +++ b/src/modules/spacecraft/module.yaml @@ -1,9 +1,8 @@ __max_num_mc_motors: &max_num_mc_motors 12 __max_num_thrusters: &max_num_thrusters 12 __max_num_servos: &max_num_servos 8 -__max_num_tilts: &max_num_tilts 4 -module_name: Control Allocation +module_name: Spacecraft parameters: - group: Geometry @@ -172,6 +171,8 @@ parameters: max: 100 default: 6.5 + + # Mixer mixer: actuator_types: diff --git a/src/modules/spacecraft/SpacecraftRateControl/sc_rate_control_params.c b/src/modules/spacecraft/spacecraft_params.c similarity index 88% rename from src/modules/spacecraft/SpacecraftRateControl/sc_rate_control_params.c rename to src/modules/spacecraft/spacecraft_params.c index f3b194f3b7..4afb818a42 100644 --- a/src/modules/spacecraft/SpacecraftRateControl/sc_rate_control_params.c +++ b/src/modules/spacecraft/spacecraft_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 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 @@ -32,11 +32,10 @@ ****************************************************************************/ /** - * @file mc_rate_control_params.c - * Parameters for multicopter attitude controller. + * @file spacecraft_params.c + * Parameters for spacecraft vehicle type. * - * @author Lorenz Meier - * @author Anton Babushkin + * @author Pedro Roque */ /** @@ -48,7 +47,7 @@ * @max 0.5 * @decimal 3 * @increment 0.01 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ROLLRATE_P, 0.15f); @@ -60,7 +59,7 @@ PARAM_DEFINE_FLOAT(SC_ROLLRATE_P, 0.15f); * @min 0.0 * @decimal 3 * @increment 0.01 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ROLLRATE_I, 0.2f); @@ -72,7 +71,7 @@ PARAM_DEFINE_FLOAT(SC_ROLLRATE_I, 0.2f); * @min 0.0 * @decimal 2 * @increment 0.01 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_RR_INT_LIM, 0.30f); @@ -85,7 +84,7 @@ PARAM_DEFINE_FLOAT(SC_RR_INT_LIM, 0.30f); * @max 0.01 * @decimal 4 * @increment 0.0005 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ROLLRATE_D, 0.003f); @@ -96,7 +95,7 @@ PARAM_DEFINE_FLOAT(SC_ROLLRATE_D, 0.003f); * * @min 0.0 * @decimal 4 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ROLLRATE_FF, 0.0f); @@ -116,7 +115,7 @@ PARAM_DEFINE_FLOAT(SC_ROLLRATE_FF, 0.0f); * @max 5.0 * @decimal 4 * @increment 0.0005 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ROLLRATE_K, 1.0f); @@ -129,7 +128,7 @@ PARAM_DEFINE_FLOAT(SC_ROLLRATE_K, 1.0f); * @max 0.6 * @decimal 3 * @increment 0.01 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_PITCHRATE_P, 0.15f); @@ -141,7 +140,7 @@ PARAM_DEFINE_FLOAT(SC_PITCHRATE_P, 0.15f); * @min 0.0 * @decimal 3 * @increment 0.01 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_PITCHRATE_I, 0.2f); @@ -153,7 +152,7 @@ PARAM_DEFINE_FLOAT(SC_PITCHRATE_I, 0.2f); * @min 0.0 * @decimal 2 * @increment 0.01 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_PR_INT_LIM, 0.30f); @@ -165,7 +164,7 @@ PARAM_DEFINE_FLOAT(SC_PR_INT_LIM, 0.30f); * @min 0.0 * @decimal 4 * @increment 0.0005 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_PITCHRATE_D, 0.003f); @@ -176,7 +175,7 @@ PARAM_DEFINE_FLOAT(SC_PITCHRATE_D, 0.003f); * * @min 0.0 * @decimal 4 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_PITCHRATE_FF, 0.0f); @@ -196,7 +195,7 @@ PARAM_DEFINE_FLOAT(SC_PITCHRATE_FF, 0.0f); * @max 5.0 * @decimal 4 * @increment 0.0005 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_PITCHRATE_K, 1.0f); @@ -209,7 +208,7 @@ PARAM_DEFINE_FLOAT(SC_PITCHRATE_K, 1.0f); * @max 10.0 * @decimal 2 * @increment 0.01 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_YAWRATE_P, 10.0f); @@ -221,7 +220,7 @@ PARAM_DEFINE_FLOAT(SC_YAWRATE_P, 10.0f); * @min 0.0 * @decimal 2 * @increment 0.01 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_YAWRATE_I, 0.865f); @@ -233,7 +232,7 @@ PARAM_DEFINE_FLOAT(SC_YAWRATE_I, 0.865f); * @min 0.0 * @decimal 2 * @increment 0.01 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_YR_INT_LIM, 0.2f); @@ -245,7 +244,7 @@ PARAM_DEFINE_FLOAT(SC_YR_INT_LIM, 0.2f); * @min 0.0 * @decimal 2 * @increment 0.01 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_YAWRATE_D, 0.0f); @@ -257,7 +256,7 @@ PARAM_DEFINE_FLOAT(SC_YAWRATE_D, 0.0f); * @min 0.0 * @decimal 4 * @increment 0.01 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_YAWRATE_FF, 0.0f); @@ -277,7 +276,7 @@ PARAM_DEFINE_FLOAT(SC_YAWRATE_FF, 0.0f); * @max 5.0 * @decimal 4 * @increment 0.0005 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_YAWRATE_K, 1.0f); @@ -291,7 +290,7 @@ PARAM_DEFINE_FLOAT(SC_YAWRATE_K, 1.0f); * @max 1800.0 * @decimal 1 * @increment 5 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ACRO_R_MAX, 720.0f); @@ -305,7 +304,7 @@ PARAM_DEFINE_FLOAT(SC_ACRO_R_MAX, 720.0f); * @max 1800.0 * @decimal 1 * @increment 5 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ACRO_P_MAX, 720.0f); @@ -319,7 +318,7 @@ PARAM_DEFINE_FLOAT(SC_ACRO_P_MAX, 720.0f); * @max 1800.0 * @decimal 1 * @increment 5 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ACRO_Y_MAX, 540.0f); @@ -334,7 +333,7 @@ PARAM_DEFINE_FLOAT(SC_ACRO_Y_MAX, 540.0f); * @min 0 * @max 1 * @decimal 2 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ACRO_EXPO, 0.69f); @@ -349,7 +348,7 @@ PARAM_DEFINE_FLOAT(SC_ACRO_EXPO, 0.69f); * @min 0 * @max 1 * @decimal 2 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ACRO_EXPO_Y, 0.69f); @@ -365,7 +364,7 @@ PARAM_DEFINE_FLOAT(SC_ACRO_EXPO_Y, 0.69f); * @min 0 * @max 0.95 * @decimal 2 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPO, 0.7f); @@ -381,7 +380,7 @@ PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPO, 0.7f); * @min 0 * @max 0.95 * @decimal 2 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPOY, 0.7f); @@ -395,7 +394,7 @@ PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPOY, 0.7f); * it will still be 0.5 at 60% battery. * * @boolean - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_INT32(SC_BAT_SCALE_EN, 0); @@ -406,7 +405,7 @@ PARAM_DEFINE_INT32(SC_BAT_SCALE_EN, 0); * @min 0 * @max 1.0 * @decimal 2 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_MAN_F_MAX, 1.0f); @@ -417,6 +416,6 @@ PARAM_DEFINE_FLOAT(SC_MAN_F_MAX, 1.0f); * @min 0 * @max 1.0 * @decimal 2 - * @group Multicopter Rate Control + * @group Spacecraft Rate Control */ PARAM_DEFINE_FLOAT(SC_MAN_T_MAX, 1.0f);