mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 01:13:34 +08:00
rft: initial merging of controllers for spacecraft vehicles
This commit is contained in:
Pedro-Roque
@@ -0,0 +1,94 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file AttitudeControl.cpp
|
||||
*/
|
||||
|
||||
#include <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;
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file AttitudeControl.hpp
|
||||
*
|
||||
* A quaternion based attitude controller.
|
||||
*
|
||||
* @author Matthias Grob <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
|
||||
};
|
||||
@@ -0,0 +1,70 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file AttitudeControlMath.hpp
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(SpacecraftAttitudeControl
|
||||
AttitudeControl.cpp
|
||||
AttitudeControl.hpp
|
||||
AttitudeControlMath.hpp
|
||||
)
|
||||
target_compile_options(SpacecraftAttitudeControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
|
||||
target_include_directories(SpacecraftAttitudeControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
# TODO: Add unit tests
|
||||
# px4_add_unit_gtest(SRC ScAttitudeControlTest.cpp LINKLIBS SpacecraftAttitudeControl)
|
||||
# px4_add_unit_gtest(SRC ScAttitudeControlMathTest.cpp LINKLIBS SpacecraftAttitudeControl)
|
||||
@@ -0,0 +1,92 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <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()));
|
||||
}
|
||||
@@ -0,0 +1,140 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <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());
|
||||
}
|
||||
@@ -34,16 +34,50 @@
|
||||
/**
|
||||
* @file SpacecraftHandler.cpp
|
||||
*
|
||||
* Control allocator.
|
||||
* Spacecraft control handler.
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
* @author Pedro Roque <padr@kth.se>
|
||||
*/
|
||||
|
||||
#include "SpacecraftHandler.hpp"
|
||||
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
SpacecraftHandler::SpacecraftHandler() :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
|
||||
{
|
||||
updateParams();
|
||||
}
|
||||
|
||||
bool SpacecraftHandler::init()
|
||||
{
|
||||
ScheduleOnInterval(4_ms); // 250 Hz
|
||||
return true;
|
||||
}
|
||||
|
||||
int SpacecraftHandler::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
return 0;
|
||||
SpacecraftHandler *instance = new SpacecraftHandler();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int SpacecraftHandler::print_status()
|
||||
@@ -75,6 +109,7 @@ int SpacecraftHandler::print_usage(const char *reason)
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("spacecraft", "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_COMMAND("status");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
|
||||
@@ -68,7 +68,7 @@ public:
|
||||
|
||||
SpacecraftHandler();
|
||||
|
||||
virtual ~SpacecraftHandler();
|
||||
~SpacecraftHandler() override = default;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
@@ -82,6 +82,12 @@ public:
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
|
||||
private: /**< loop duration performance counter */
|
||||
bool init();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Update the parameters of the module.
|
||||
*/
|
||||
void updateParams() override;
|
||||
|
||||
};
|
||||
|
||||
@@ -0,0 +1,44 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(SpacecraftPositionControl
|
||||
ControlMath.cpp
|
||||
ControlMath.hpp
|
||||
PositionControl.cpp
|
||||
PositionControl.hpp
|
||||
)
|
||||
target_include_directories(SpacecraftPositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
# TODO: add unit tests
|
||||
# px4_add_unit_gtest(SRC ScControlMathTest.cpp LINKLIBS SpacecraftPositionControl)
|
||||
# px4_add_unit_gtest(SRC ScPositionControlTest.cpp LINKLIBS SpacecraftPositionControl)
|
||||
@@ -0,0 +1,260 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018 - 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ControlMath.cpp
|
||||
*/
|
||||
|
||||
#include "ControlMath.hpp"
|
||||
#include <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());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,118 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018 - 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ControlMath.hpp
|
||||
*
|
||||
* Simple functions for vector manipulation that do not fit into matrix lib.
|
||||
* These functions are specific for controls.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <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);
|
||||
}
|
||||
@@ -0,0 +1,220 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 - 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file PositionControl.cpp
|
||||
*/
|
||||
|
||||
#include "PositionControl.hpp"
|
||||
#include "ControlMath.hpp"
|
||||
#include <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];
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,205 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 - 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file PositionControl.hpp
|
||||
*
|
||||
* A cascaded position controller for position/velocity control only.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <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 */
|
||||
};
|
||||
@@ -0,0 +1,258 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <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);
|
||||
}
|
||||
@@ -0,0 +1,368 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <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);
|
||||
}
|
||||
@@ -0,0 +1,281 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "SpacecraftRateControl.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <circuit_breaker/circuit_breaker.h>
|
||||
#include <mathlib/math/Limits.hpp>
|
||||
#include <mathlib/math/Functions.hpp>
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
using namespace matrix;
|
||||
using namespace time_literals;
|
||||
using math::radians;
|
||||
|
||||
SpacecraftRateControl::SpacecraftRateControl(ModuleParams *parent) : ModuleParams(parent)
|
||||
{
|
||||
_controller_status_pub.advertise();
|
||||
updateParams();
|
||||
}
|
||||
|
||||
void SpacecraftRateControl::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
// rate control parameters
|
||||
// The controller gain K is used to convert the parallel (P + I/s + sD) form
|
||||
// to the ideal (K * [1 + 1/sTi + sTd]) form
|
||||
const Vector3f rate_k = Vector3f(_param_sc_rollrate_k.get(), _param_sc_pitchrate_k.get(), _param_sc_yawrate_k.get());
|
||||
|
||||
_rate_control.setPidGains(
|
||||
rate_k.emult(Vector3f(_param_sc_rollrate_p.get(), _param_sc_pitchrate_p.get(), _param_sc_yawrate_p.get())),
|
||||
rate_k.emult(Vector3f(_param_sc_rollrate_i.get(), _param_sc_pitchrate_i.get(), _param_sc_yawrate_i.get())),
|
||||
rate_k.emult(Vector3f(_param_sc_rollrate_d.get(), _param_sc_pitchrate_d.get(), _param_sc_yawrate_d.get())));
|
||||
|
||||
_rate_control.setIntegratorLimit(
|
||||
Vector3f(_param_sc_rr_int_lim.get(), _param_sc_pr_int_lim.get(), _param_sc_yr_int_lim.get()));
|
||||
|
||||
_rate_control.setFeedForwardGain(
|
||||
Vector3f(_param_sc_rollrate_ff.get(), _param_sc_pitchrate_ff.get(), _param_sc_yawrate_ff.get()));
|
||||
|
||||
// manual rate control acro mode rate limits
|
||||
_acro_rate_max = Vector3f(radians(_param_sc_acro_r_max.get()), radians(_param_sc_acro_p_max.get()),
|
||||
radians(_param_sc_acro_y_max.get()));
|
||||
_manual_force_max = _param_sc_manual_f_max.get();
|
||||
_manual_torque_max = _param_sc_manual_t_max.get();
|
||||
}
|
||||
|
||||
void SpacecraftRateControl::updateRateControl()
|
||||
{
|
||||
if (_vehicle_angular_velocity_sub.update(&angular_velocity))
|
||||
{
|
||||
const hrt_abstime now = angular_velocity.timestamp_sample;
|
||||
|
||||
// Guard against too small (< 0.125ms) and too large (> 20ms) dt's.
|
||||
const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f);
|
||||
_last_run = now;
|
||||
|
||||
const Vector3f rates{angular_velocity.xyz};
|
||||
const Vector3f angular_accel{angular_velocity.xyz_derivative};
|
||||
|
||||
/* check for updates in other topics */
|
||||
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
|
||||
|
||||
if (_vehicle_land_detected_sub.updated()) {
|
||||
vehicle_land_detected_s vehicle_land_detected;
|
||||
|
||||
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
|
||||
_landed = vehicle_land_detected.landed;
|
||||
_maybe_landed = vehicle_land_detected.maybe_landed;
|
||||
}
|
||||
}
|
||||
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
|
||||
// use rates setpoint topic
|
||||
vehicle_rates_setpoint_s vehicle_rates_setpoint{};
|
||||
|
||||
if (_vehicle_control_mode.flag_control_manual_enabled &&
|
||||
!_vehicle_control_mode.flag_control_attitude_enabled) {
|
||||
// Here we can be in: Manual Mode or Acro Mode
|
||||
// generate the rate setpoint from sticks
|
||||
manual_control_setpoint_s manual_control_setpoint;
|
||||
|
||||
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
|
||||
if (_vehicle_control_mode.flag_control_rates_enabled) {
|
||||
// manual rates control - ACRO mode
|
||||
const Vector3f man_rate_sp{manual_control_setpoint.roll,
|
||||
-manual_control_setpoint.pitch,
|
||||
manual_control_setpoint.yaw};
|
||||
|
||||
_rates_setpoint = man_rate_sp * 5;
|
||||
_thrust_setpoint(2) = -manual_control_setpoint.throttle;
|
||||
_thrust_setpoint(0) = _thrust_setpoint(1) = 0.f;
|
||||
|
||||
// publish rate setpoint
|
||||
vehicle_rates_setpoint.roll = _rates_setpoint(0);
|
||||
vehicle_rates_setpoint.pitch = _rates_setpoint(1);
|
||||
vehicle_rates_setpoint.yaw = _rates_setpoint(2);
|
||||
_thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body);
|
||||
vehicle_rates_setpoint.timestamp = hrt_absolute_time();
|
||||
|
||||
_vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint);
|
||||
|
||||
} else if (!_vehicle_control_mode.flag_control_rates_enabled) {
|
||||
// Manual/direct control
|
||||
// Yaw stick commands rotational moment, Roll/Pitch stick commands translational forces
|
||||
// All other axis are set as zero (We only have four channels on the manual control inputs)
|
||||
_thrust_setpoint(0) = math::constrain((manual_control_setpoint.pitch * _manual_force_max), -1.f, 1.f);
|
||||
_thrust_setpoint(1) = math::constrain((manual_control_setpoint.roll * _manual_force_max), -1.f, 1.f);
|
||||
_thrust_setpoint(2) = 0.0;
|
||||
|
||||
_torque_setpoint(0) = _torque_setpoint(1) = 0.0;
|
||||
_torque_setpoint(2) = math::constrain((manual_control_setpoint.yaw * _manual_torque_max), -1.f, 1.f);
|
||||
|
||||
// publish thrust and torque setpoints
|
||||
vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
|
||||
vehicle_torque_setpoint_s vehicle_torque_setpoint{};
|
||||
|
||||
_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
|
||||
_torque_setpoint.copyTo(vehicle_torque_setpoint.xyz);
|
||||
|
||||
vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
|
||||
vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
|
||||
|
||||
vehicle_torque_setpoint.timestamp = hrt_absolute_time();
|
||||
vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
|
||||
|
||||
_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
|
||||
_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
|
||||
|
||||
updateActuatorControlsStatus(vehicle_torque_setpoint, dt);
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
|
||||
// Get rates from other controllers (e.g. position or attitude controller)
|
||||
if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) {
|
||||
_rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0);
|
||||
_rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1);
|
||||
_rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2);
|
||||
_thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body);
|
||||
}
|
||||
}
|
||||
|
||||
// run the rate controller
|
||||
if (_vehicle_control_mode.flag_control_rates_enabled) {
|
||||
// reset integral if disarmed
|
||||
if (!_vehicle_control_mode.flag_armed ||
|
||||
_vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_SPACECRAFT) {
|
||||
_rate_control.resetIntegral();
|
||||
}
|
||||
|
||||
// update saturation status from control allocation feedback
|
||||
control_allocator_status_s control_allocator_status;
|
||||
|
||||
if (_control_allocator_status_sub.update(&control_allocator_status)) {
|
||||
Vector<bool, 3> saturation_positive;
|
||||
Vector<bool, 3> saturation_negative;
|
||||
|
||||
if (!control_allocator_status.torque_setpoint_achieved) {
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
|
||||
saturation_positive(i) = true;
|
||||
|
||||
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
|
||||
saturation_negative(i) = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: send the unallocated value directly for better anti-windup
|
||||
// _rate_control.setSaturationStatus(saturation_positive, saturation_negative);
|
||||
}
|
||||
|
||||
// run rate controller
|
||||
const Vector3f torque_sp =
|
||||
_rate_control.update(rates, _rates_setpoint, angular_accel, dt, false);
|
||||
// PX4_INFO("Rate: %f %f %f", (double)rates(0), (double)rates(1), (double)rates(2));
|
||||
// PX4_INFO("Rate Setpoint: %f %f %f", (double)_rates_setpoint(0), (double)_rates_setpoint(1), (double)_rates_setpoint(2));
|
||||
// PX4_INFO("Torque sp: %f %f %f", (double)torque_sp(0), (double)torque_sp(1), (double)torque_sp(2));
|
||||
|
||||
// publish rate controller status
|
||||
rate_ctrl_status_s rate_ctrl_status{};
|
||||
_rate_control.getRateControlStatus(rate_ctrl_status);
|
||||
rate_ctrl_status.timestamp = hrt_absolute_time();
|
||||
_controller_status_pub.publish(rate_ctrl_status);
|
||||
|
||||
// publish thrust and torque setpoints
|
||||
vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
|
||||
vehicle_torque_setpoint_s vehicle_torque_setpoint{};
|
||||
|
||||
_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
|
||||
vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(torque_sp(0)) ? torque_sp(0) : 0.f;
|
||||
vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(torque_sp(1)) ? torque_sp(1) : 0.f;
|
||||
vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(torque_sp(2)) ? torque_sp(2) : 0.f;
|
||||
|
||||
// scale setpoints by battery status if enabled
|
||||
if (_param_sc_bat_scale_en.get()) {
|
||||
if (_battery_status_sub.updated()) {
|
||||
battery_status_s battery_status;
|
||||
|
||||
if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
|
||||
_battery_status_scale = battery_status.scale;
|
||||
}
|
||||
}
|
||||
|
||||
if (_battery_status_scale > 0.f) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
vehicle_thrust_setpoint.xyz[i] =
|
||||
math::constrain(vehicle_thrust_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f);
|
||||
vehicle_torque_setpoint.xyz[i] =
|
||||
math::constrain(vehicle_torque_setpoint.xyz[i] * _battery_status_scale, -1.f, 1.f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
|
||||
vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
|
||||
_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
|
||||
|
||||
vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
|
||||
vehicle_torque_setpoint.timestamp = hrt_absolute_time();
|
||||
_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
|
||||
|
||||
updateActuatorControlsStatus(vehicle_torque_setpoint, dt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SpacecraftRateControl::updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint,
|
||||
float dt)
|
||||
{
|
||||
for (int i = 0; i < 3; i++) {
|
||||
_control_energy[i] += vehicle_torque_setpoint.xyz[i] * vehicle_torque_setpoint.xyz[i] * dt;
|
||||
}
|
||||
|
||||
_energy_integration_time += dt;
|
||||
|
||||
if (_energy_integration_time > 500e-3f) {
|
||||
actuator_controls_status_s status;
|
||||
status.timestamp = vehicle_torque_setpoint.timestamp;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
status.control_power[i] = _control_energy[i] / _energy_integration_time;
|
||||
_control_energy[i] = 0.f;
|
||||
}
|
||||
|
||||
_actuator_controls_status_pub.publish(status);
|
||||
_energy_integration_time = 0.f;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,170 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <lib/rate_control/rate_control.hpp>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/systemlib/mavlink_log.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/actuator_controls_status.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/control_allocator_status.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/rate_ctrl_status.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_thrust_setpoint.h>
|
||||
#include <uORB/topics/vehicle_torque_setpoint.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class SpacecraftRateControl : public ModuleBase<SpacecraftRateControl>, public ModuleParams, public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
SpacecraftRateControl(ModuleParams *parent);
|
||||
~SpacecraftRateControl() = default;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
bool init();
|
||||
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
/**
|
||||
* initialize some vectors/matrices from parameters
|
||||
*/
|
||||
void parameters_updated();
|
||||
|
||||
void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt);
|
||||
|
||||
RateControl _rate_control; ///< class for rate control calculations
|
||||
|
||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||
uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vehicle_rates_setpoint_sub{ORB_ID(vehicle_rates_setpoint)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
|
||||
|
||||
uORB::Publication<actuator_controls_status_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{};
|
||||
|
||||
bool _landed{true};
|
||||
bool _maybe_landed{true};
|
||||
|
||||
hrt_abstime _last_run{0};
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop duration performance counter */
|
||||
|
||||
// keep setpoint values between updates
|
||||
matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */
|
||||
matrix::Vector3f _rates_setpoint{};
|
||||
float _manual_torque_max{1.0};
|
||||
float _manual_force_max{1.0};
|
||||
|
||||
float _battery_status_scale{0.0f};
|
||||
matrix::Vector3f _thrust_setpoint{};
|
||||
matrix::Vector3f _torque_setpoint{};
|
||||
|
||||
float _energy_integration_time{0.0f};
|
||||
float _control_energy[4] {};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::SC_ROLLRATE_P>) _param_sc_rollrate_p,
|
||||
(ParamFloat<px4::params::SC_ROLLRATE_I>) _param_sc_rollrate_i,
|
||||
(ParamFloat<px4::params::SC_RR_INT_LIM>) _param_sc_rr_int_lim,
|
||||
(ParamFloat<px4::params::SC_ROLLRATE_D>) _param_sc_rollrate_d,
|
||||
(ParamFloat<px4::params::SC_ROLLRATE_FF>) _param_sc_rollrate_ff,
|
||||
(ParamFloat<px4::params::SC_ROLLRATE_K>) _param_sc_rollrate_k,
|
||||
|
||||
(ParamFloat<px4::params::SC_PITCHRATE_P>) _param_sc_pitchrate_p,
|
||||
(ParamFloat<px4::params::SC_PITCHRATE_I>) _param_sc_pitchrate_i,
|
||||
(ParamFloat<px4::params::SC_PR_INT_LIM>) _param_sc_pr_int_lim,
|
||||
(ParamFloat<px4::params::SC_PITCHRATE_D>) _param_sc_pitchrate_d,
|
||||
(ParamFloat<px4::params::SC_PITCHRATE_FF>) _param_sc_pitchrate_ff,
|
||||
(ParamFloat<px4::params::SC_PITCHRATE_K>) _param_sc_pitchrate_k,
|
||||
|
||||
(ParamFloat<px4::params::SC_YAWRATE_P>) _param_sc_yawrate_p,
|
||||
(ParamFloat<px4::params::SC_YAWRATE_I>) _param_sc_yawrate_i,
|
||||
(ParamFloat<px4::params::SC_YR_INT_LIM>) _param_sc_yr_int_lim,
|
||||
(ParamFloat<px4::params::SC_YAWRATE_D>) _param_sc_yawrate_d,
|
||||
(ParamFloat<px4::params::SC_YAWRATE_FF>) _param_sc_yawrate_ff,
|
||||
(ParamFloat<px4::params::SC_YAWRATE_K>) _param_sc_yawrate_k,
|
||||
|
||||
(ParamFloat<px4::params::SC_ACRO_R_MAX>) _param_sc_acro_r_max,
|
||||
(ParamFloat<px4::params::SC_ACRO_P_MAX>) _param_sc_acro_p_max,
|
||||
(ParamFloat<px4::params::SC_ACRO_Y_MAX>) _param_sc_acro_y_max,
|
||||
(ParamFloat<px4::params::SC_ACRO_EXPO>) _param_sc_acro_expo, /**< expo stick curve shape (roll & pitch) */
|
||||
(ParamFloat<px4::params::SC_ACRO_EXPO_Y>) _param_sc_acro_expo_y, /**< expo stick curve shape (yaw) */
|
||||
(ParamFloat<px4::params::SC_ACRO_SUPEXPO>) _param_sc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */
|
||||
(ParamFloat<px4::params::SC_ACRO_SUPEXPOY>) _param_sc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */
|
||||
|
||||
(ParamFloat<px4::params::SC_MAN_F_MAX>) _param_sc_manual_f_max,
|
||||
(ParamFloat<px4::params::SC_MAN_T_MAX>) _param_sc_manual_t_max,
|
||||
|
||||
(ParamBool<px4::params::SC_BAT_SCALE_EN>) _param_sc_bat_scale_en
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,422 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mc_rate_control_params.c
|
||||
* Parameters for multicopter attitude controller.
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
* @author Anton Babushkin <anton@px4.io>
|
||||
*/
|
||||
|
||||
/**
|
||||
* Roll rate P gain
|
||||
*
|
||||
* Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.01
|
||||
* @max 0.5
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ROLLRATE_P, 0.15f);
|
||||
|
||||
/**
|
||||
* Roll rate I gain
|
||||
*
|
||||
* Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ROLLRATE_I, 0.2f);
|
||||
|
||||
/**
|
||||
* Roll rate integrator limit
|
||||
*
|
||||
* Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_RR_INT_LIM, 0.30f);
|
||||
|
||||
/**
|
||||
* Roll rate D gain
|
||||
*
|
||||
* Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 0.01
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ROLLRATE_D, 0.003f);
|
||||
|
||||
/**
|
||||
* Roll rate feedforward
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ROLLRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Roll rate controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = SC_ROLLRATE_K * (SC_ROLLRATE_P * error
|
||||
* + SC_ROLLRATE_I * error_integral
|
||||
* + SC_ROLLRATE_D * error_derivative)
|
||||
* Set SC_ROLLRATE_P=1 to implement a PID in the ideal form.
|
||||
* Set SC_ROLLRATE_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.01
|
||||
* @max 5.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ROLLRATE_K, 1.0f);
|
||||
|
||||
/**
|
||||
* Pitch rate P gain
|
||||
*
|
||||
* Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.01
|
||||
* @max 0.6
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_PITCHRATE_P, 0.15f);
|
||||
|
||||
/**
|
||||
* Pitch rate I gain
|
||||
*
|
||||
* Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 3
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_PITCHRATE_I, 0.2f);
|
||||
|
||||
/**
|
||||
* Pitch rate integrator limit
|
||||
*
|
||||
* Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_PR_INT_LIM, 0.30f);
|
||||
|
||||
/**
|
||||
* Pitch rate D gain
|
||||
*
|
||||
* Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_PITCHRATE_D, 0.003f);
|
||||
|
||||
/**
|
||||
* Pitch rate feedforward
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_PITCHRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Pitch rate controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = SC_PITCHRATE_K * (SC_PITCHRATE_P * error
|
||||
* + SC_PITCHRATE_I * error_integral
|
||||
* + SC_PITCHRATE_D * error_derivative)
|
||||
* Set SC_PITCHRATE_P=1 to implement a PID in the ideal form.
|
||||
* Set SC_PITCHRATE_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.01
|
||||
* @max 5.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_PITCHRATE_K, 1.0f);
|
||||
|
||||
/**
|
||||
* Yaw rate P gain
|
||||
*
|
||||
* Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_YAWRATE_P, 10.0f);
|
||||
|
||||
/**
|
||||
* Yaw rate I gain
|
||||
*
|
||||
* Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_YAWRATE_I, 0.865f);
|
||||
|
||||
/**
|
||||
* Yaw rate integrator limit
|
||||
*
|
||||
* Yaw rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_YR_INT_LIM, 0.2f);
|
||||
|
||||
/**
|
||||
* Yaw rate D gain
|
||||
*
|
||||
* Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_YAWRATE_D, 0.0f);
|
||||
|
||||
/**
|
||||
* Yaw rate feedforward
|
||||
*
|
||||
* Improves tracking performance.
|
||||
*
|
||||
* @min 0.0
|
||||
* @decimal 4
|
||||
* @increment 0.01
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_YAWRATE_FF, 0.0f);
|
||||
|
||||
/**
|
||||
* Yaw rate controller gain
|
||||
*
|
||||
* Global gain of the controller.
|
||||
*
|
||||
* This gain scales the P, I and D terms of the controller:
|
||||
* output = SC_YAWRATE_K * (SC_YAWRATE_P * error
|
||||
* + SC_YAWRATE_I * error_integral
|
||||
* + SC_YAWRATE_D * error_derivative)
|
||||
* Set SC_YAWRATE_P=1 to implement a PID in the ideal form.
|
||||
* Set SC_YAWRATE_K=1 to implement a PID in the parallel form.
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 5.0
|
||||
* @decimal 4
|
||||
* @increment 0.0005
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_YAWRATE_K, 1.0f);
|
||||
|
||||
/**
|
||||
* Max acro roll rate
|
||||
*
|
||||
* default: 2 turns per second
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 1800.0
|
||||
* @decimal 1
|
||||
* @increment 5
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ACRO_R_MAX, 720.0f);
|
||||
|
||||
/**
|
||||
* Max acro pitch rate
|
||||
*
|
||||
* default: 2 turns per second
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 1800.0
|
||||
* @decimal 1
|
||||
* @increment 5
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ACRO_P_MAX, 720.0f);
|
||||
|
||||
/**
|
||||
* Max acro yaw rate
|
||||
*
|
||||
* default 1.5 turns per second
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0.0
|
||||
* @max 1800.0
|
||||
* @decimal 1
|
||||
* @increment 5
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ACRO_Y_MAX, 540.0f);
|
||||
|
||||
/**
|
||||
* Acro mode Expo factor for Roll and Pitch.
|
||||
*
|
||||
* Exponential factor for tuning the input curve shape.
|
||||
*
|
||||
* 0 Purely linear input curve
|
||||
* 1 Purely cubic input curve
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 2
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ACRO_EXPO, 0.69f);
|
||||
|
||||
/**
|
||||
* Acro mode Expo factor for Yaw.
|
||||
*
|
||||
* Exponential factor for tuning the input curve shape.
|
||||
*
|
||||
* 0 Purely linear input curve
|
||||
* 1 Purely cubic input curve
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 2
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ACRO_EXPO_Y, 0.69f);
|
||||
|
||||
/**
|
||||
* Acro mode SuperExpo factor for Roll and Pitch.
|
||||
*
|
||||
* SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO.
|
||||
*
|
||||
* 0 Pure Expo function
|
||||
* 0.7 reasonable shape enhancement for intuitive stick feel
|
||||
* 0.95 very strong bent input curve only near maxima have effect
|
||||
*
|
||||
* @min 0
|
||||
* @max 0.95
|
||||
* @decimal 2
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPO, 0.7f);
|
||||
|
||||
/**
|
||||
* Acro mode SuperExpo factor for Yaw.
|
||||
*
|
||||
* SuperExpo factor for refining the input curve shape tuned using SC_ACRO_EXPO_Y.
|
||||
*
|
||||
* 0 Pure Expo function
|
||||
* 0.7 reasonable shape enhancement for intuitive stick feel
|
||||
* 0.95 very strong bent input curve only near maxima have effect
|
||||
*
|
||||
* @min 0
|
||||
* @max 0.95
|
||||
* @decimal 2
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_ACRO_SUPEXPOY, 0.7f);
|
||||
|
||||
/**
|
||||
* Battery power level scaler
|
||||
*
|
||||
* This compensates for voltage drop of the battery over time by attempting to
|
||||
* normalize performance across the operating range of the battery. The copter
|
||||
* should constantly behave as if it was fully charged with reduced max acceleration
|
||||
* at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery,
|
||||
* it will still be 0.5 at 60% battery.
|
||||
*
|
||||
* @boolean
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SC_BAT_SCALE_EN, 0);
|
||||
|
||||
/**
|
||||
* Manual mode maximum force.
|
||||
*
|
||||
* *
|
||||
* @min 0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_MAN_F_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Manual mode maximum torque.
|
||||
*
|
||||
* *
|
||||
* @min 0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
* @group Multicopter Rate Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SC_MAN_T_MAX, 1.0f);
|
||||
Reference in New Issue
Block a user