fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 00:14:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat: rate controller nominal

Browse Source
This commit is contained in:
Pedro-Roque 2025-04-13 14:51:38 +02:00
parent 95123b88f6
commit de9755b33b
24 changed files with 117 additions and 2065 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d
View File

@ -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

2
ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d
View File

@ -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

4
ROMFS/px4fmu_common/init.d/rc.sc_defaults
View File

@ -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

9
ROMFS/px4fmu_common/init.d/rc.vehicle_setup
View File

@ -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.
#

2
src/modules/spacecraft/CMakeLists.txt
View File

@ -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
)

94
src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.cpp
View File

@ -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 <AttitudeControl.hpp>
#include <mathlib/math/Functions.hpp>
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;
}

105
src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControl.hpp
View File

@ -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 <maetugr@gmail.com>
*
* 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 <matrix/matrix/math.hpp>
#include <mathlib/math/Limits.hpp>
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
};

70
src/modules/spacecraft/SpacecraftAttitudeControl/AttitudeControlMath.hpp
View File

@ -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 <matrix/matrix/math.hpp>
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);
}
}

44
src/modules/spacecraft/SpacecraftAttitudeControl/CMakeLists.txt
View File

@ -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)

92
src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlMathTest.cpp
View File

@ -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 <gtest/gtest.h>
#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()));
}

140
src/modules/spacecraft/SpacecraftAttitudeControl/ScAttitudeControlTest.cpp
View File

@ -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 <gtest/gtest.h>
#include <AttitudeControl.hpp>
#include <mathlib/math/Functions.hpp>
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());
}

25
src/modules/spacecraft/SpacecraftHandler.cpp
View File

@ -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();

22
src/modules/spacecraft/SpacecraftHandler.hpp
View File

@ -62,6 +62,9 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/failure_detector_status.h>
// Local includes
#include "SpacecraftRateControl/SpacecraftRateControl.hpp"
class SpacecraftHandler : public ModuleBase<SpacecraftHandler>, 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_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
// Class instances
SpacecraftRateControl _spacecraft_rate_control{this};
// Variables
hrt_abstime _timestamp{0};
float _dt{0.f};
};

260
src/modules/spacecraft/SpacecraftPositionControl/ControlMath.cpp
View File

@ -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 <px4_platform_common/defines.h>
#include <float.h>
#include <mathlib/mathlib.h>
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());
}
}

118
src/modules/spacecraft/SpacecraftPositionControl/ControlMath.hpp
View File

@ -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 <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
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);
}

220
src/modules/spacecraft/SpacecraftPositionControl/PositionControl.cpp
View File

@ -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 <float.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
#include <geo/geo.h>
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];
}
}

205
src/modules/spacecraft/SpacecraftPositionControl/PositionControl.hpp
View File

@ -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 <lib/mathlib/mathlib.h>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
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 */
};

258
src/modules/spacecraft/SpacecraftPositionControl/ScControlMathTest.cpp
View File

@ -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 <cmath>
#include <gtest/gtest.h>
#include <ControlMath.hpp>
#include <px4_platform_common/defines.h>
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);
}

368
src/modules/spacecraft/SpacecraftPositionControl/ScPositionControlTest.cpp
View File

@ -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 <gtest/gtest.h>
#include <PositionControl.hpp>
#include <px4_defines.h>
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<float>(combination) / static_cast<float>(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);
}

17
src/modules/spacecraft/SpacecraftPositionControl/CMakeLists.txt → src/modules/spacecraft/SpacecraftRateControl/CMakeLists.txt
View File

@ -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})

23
src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.cpp
View File

@ -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{};

32
src/modules/spacecraft/SpacecraftRateControl/SpacecraftRateControl.hpp
View File

@ -62,31 +62,21 @@
using namespace time_literals;
class SpacecraftRateControl : public ModuleBase<SpacecraftRateControl>, 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_s> _actuator_controls_status_pub{ORB_ID(actuator_controls_status_0)};
uORB::PublicationMulti<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status)};
uORB::Publication<vehicle_rates_setpoint_s> _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
uORB::Publication<vehicle_thrust_setpoint_s> _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};

5
src/modules/spacecraft/module.yaml
View File

@ -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:

65
src/modules/spacecraft/SpacecraftRateControl/sc_rate_control_params.c → src/modules/spacecraft/spacecraft_params.c
View File

@ -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 <lorenz@px4.io>
* @author Anton Babushkin <anton@px4.io>
* @author Pedro Roque <padr@kth.se>
*/
/**
@ -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);