forked from Archive/PX4-Autopilot
Compare commits
5 Commits
main
...
attitude-g
Author | SHA1 | Date |
---|---|---|
Matthias Grob | 7c8dc3d229 | |
Matthias Grob | 612de1e0f3 | |
Matthias Grob | 05b4914983 | |
Matthias Grob | e21b43f5e2 | |
Matthias Grob | 75695787fd |
|
@ -1593,8 +1593,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp)
|
||||||
Quatf delta_q_reset;
|
Quatf delta_q_reset;
|
||||||
_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
|
_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
|
||||||
|
|
||||||
lpos.heading = Eulerf(_ekf.getQuaternion()).psi();
|
const Quatf q = _ekf.getQuaternion();
|
||||||
lpos.unaided_heading = _ekf.getUnaidedYaw();
|
lpos.heading = 2.f * atan2f(q(3), q(0));
|
||||||
|
lpos.unaided_heading = _ekf.getUnaidedYaw(); // TODO: Needs to change as well
|
||||||
lpos.delta_heading = Eulerf(delta_q_reset).psi();
|
lpos.delta_heading = Eulerf(delta_q_reset).psi();
|
||||||
lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
|
lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
|
||||||
|
|
||||||
|
|
|
@ -138,3 +138,31 @@ TEST(AttitudeControlTest, YawWeightScaling)
|
||||||
// THEN: no actuation (also no NAN)
|
// THEN: no actuation (also no NAN)
|
||||||
EXPECT_EQ(rate_setpoint, Vector3f());
|
EXPECT_EQ(rate_setpoint, Vector3f());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST(AttitudeControlTest, HeadingCorrectness)
|
||||||
|
{
|
||||||
|
//Quatf q(0.863f, 0.358f, 0.358f, 0.f);
|
||||||
|
Quatf q(0.733f, 0.462f, 0.191f, 0.462f);
|
||||||
|
q.normalize();
|
||||||
|
float yaw = matrix::Eulerf(q).psi();
|
||||||
|
|
||||||
|
Quatf q_red(Vector3f(0, 0, 1), q.dcm_z());
|
||||||
|
Quatf q_mix = q_red.inversed() * q;
|
||||||
|
q_mix.print();
|
||||||
|
|
||||||
|
Quatf q_mix2 = q;
|
||||||
|
q_mix2(1) = 0.f;
|
||||||
|
q_mix2(2) = 0.f;
|
||||||
|
q_mix2.normalize();
|
||||||
|
q_mix2.print();
|
||||||
|
|
||||||
|
q_mix2(3) = math::constrain(q_mix2(3), -1.f, 1.f);
|
||||||
|
float heading = 2.f * asinf(q_mix2(3));
|
||||||
|
|
||||||
|
float heading2 = 2.f * atan2f(q(3), q(0));
|
||||||
|
EXPECT_FLOAT_EQ(heading, heading2);
|
||||||
|
|
||||||
|
EXPECT_GT(fabsf(yaw), FLT_EPSILON);
|
||||||
|
EXPECT_LT(fabsf(heading), FLT_EPSILON);
|
||||||
|
EXPECT_TRUE(false);
|
||||||
|
}
|
||||||
|
|
|
@ -41,3 +41,4 @@ target_include_directories(PositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||||
|
|
||||||
px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl)
|
px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl)
|
||||||
px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl)
|
px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl)
|
||||||
|
px4_add_unit_gtest(SRC PositionAttitudeTest.cpp LINKLIBS PositionControl AttitudeControl)
|
||||||
|
|
|
@ -44,9 +44,20 @@ using namespace matrix;
|
||||||
|
|
||||||
namespace ControlMath
|
namespace ControlMath
|
||||||
{
|
{
|
||||||
|
|
||||||
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
|
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
|
||||||
{
|
{
|
||||||
bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
|
const Quatf q = bodyzToQuaternion(thr_sp, yaw_sp);
|
||||||
|
|
||||||
|
// copy quaternion setpoint to attitude setpoint topic
|
||||||
|
q.copyTo(att_sp.q_d);
|
||||||
|
|
||||||
|
// calculate euler angles, for logging only, must not be used for control
|
||||||
|
const Eulerf euler{q};
|
||||||
|
att_sp.roll_body = euler.phi();
|
||||||
|
att_sp.pitch_body = euler.theta();
|
||||||
|
att_sp.yaw_body = euler.psi();
|
||||||
|
|
||||||
att_sp.thrust_body[2] = -thr_sp.length();
|
att_sp.thrust_body[2] = -thr_sp.length();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -67,56 +78,16 @@ void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_
|
||||||
body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit();
|
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)
|
Quatf bodyzToQuaternion(Vector3f body_z, const float yaw_sp)
|
||||||
{
|
{
|
||||||
// zero vector, no direction, set safe level value
|
// zero vector, no direction, set safe level value
|
||||||
if (body_z.norm_squared() < FLT_EPSILON) {
|
if (body_z.norm_squared() < FLT_EPSILON) {
|
||||||
body_z(2) = 1.f;
|
body_z(2) = -1.f;
|
||||||
}
|
}
|
||||||
|
|
||||||
body_z.normalize();
|
const Quatf q{Vector3f(0.f, 0.f, -1.f), body_z};
|
||||||
|
const Quatf q_yaw{cosf(yaw_sp / 2.f), 0.f, 0.f, sinf(yaw_sp / 2.f)};
|
||||||
// vector of desired yaw direction in XY plane, rotated by PI/2
|
return q * q_yaw;
|
||||||
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)
|
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
|
||||||
|
|
|
@ -69,6 +69,8 @@ void limitTilt(matrix::Vector3f &body_unit, const matrix::Vector3f &world_unit,
|
||||||
*/
|
*/
|
||||||
void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
|
void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
|
||||||
|
|
||||||
|
matrix::Quatf bodyzToQuaternion(matrix::Vector3f body_z, const float yaw_sp);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Outputs the sum of two vectors but respecting the limits and priority.
|
* 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.
|
* The sum of two vectors are constraint such that v0 has priority over v1.
|
||||||
|
|
|
@ -100,37 +100,65 @@ TEST(ControlMathTest, LimitTilt10degree)
|
||||||
EXPECT_FLOAT_EQ(2.f * body(0), body(1));
|
EXPECT_FLOAT_EQ(2.f * body(0), body(1));
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(ControlMathTest, ThrottleAttitudeMapping)
|
class ControlMathAttitudeMappingTest : public ::testing::Test
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
void checkDirection(const Vector3f thrust_setpoint, const float yaw)
|
||||||
|
{
|
||||||
|
thrustToAttitude(thrust_setpoint, yaw, _attitude_setpoint);
|
||||||
|
EXPECT_EQ(Quatf(_attitude_setpoint.q_d).dcm_z(), -thrust_setpoint.normalized());
|
||||||
|
EXPECT_EQ(_attitude_setpoint.thrust_body[2], -thrust_setpoint.length());
|
||||||
|
}
|
||||||
|
|
||||||
|
void checkEuler(const float roll, const float pitch, const float yaw)
|
||||||
|
{
|
||||||
|
EXPECT_FLOAT_EQ(_attitude_setpoint.roll_body, roll);
|
||||||
|
EXPECT_FLOAT_EQ(_attitude_setpoint.pitch_body, pitch);
|
||||||
|
EXPECT_FLOAT_EQ(_attitude_setpoint.yaw_body, yaw);
|
||||||
|
}
|
||||||
|
|
||||||
|
vehicle_attitude_setpoint_s _attitude_setpoint{};
|
||||||
|
};
|
||||||
|
|
||||||
|
TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingNoRotation)
|
||||||
{
|
{
|
||||||
/* expected: zero roll, zero pitch, zero yaw, full thr mag
|
/* expected: zero roll, zero pitch, zero yaw, full thr mag
|
||||||
* reason: thrust pointing full upward */
|
* reason: thrust pointing full upward */
|
||||||
Vector3f thr{0.f, 0.f, -1.f};
|
checkDirection(Vector3f(0.f, 0.f, -1.f), 0.f);
|
||||||
float yaw = 0.f;
|
checkEuler(0.f, 0.f, 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);
|
|
||||||
|
|
||||||
|
TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingYaw90)
|
||||||
|
{
|
||||||
/* expected: same as before but with 90 yaw
|
/* expected: same as before but with 90 yaw
|
||||||
* reason: only yaw changed */
|
* reason: only yaw changed */
|
||||||
yaw = M_PI_2_F;
|
checkDirection(Vector3f(0.f, 0.f, -1.f), M_PI_2_F);
|
||||||
thrustToAttitude(thr, yaw, att);
|
checkEuler(0.f, 0.f, M_PI_2_F);
|
||||||
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);
|
|
||||||
|
|
||||||
|
TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingUpsideDown)
|
||||||
|
{
|
||||||
/* expected: same as before but roll 180
|
/* expected: same as before but roll 180
|
||||||
* reason: thrust points straight down and order Euler
|
* reason: thrust points straight down and order Euler
|
||||||
* order is: 1. roll, 2. pitch, 3. yaw */
|
* order is: 1. roll, 2. pitch, 3. yaw */
|
||||||
thr = Vector3f(0.f, 0.f, 1.f);
|
checkDirection(Vector3f(0.f, 0.f, 1.f), 0.f);
|
||||||
thrustToAttitude(thr, yaw, att);
|
checkEuler(M_PI_F, 0.f, 0.f);
|
||||||
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);
|
TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingUpsideDownYaw90)
|
||||||
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 */
|
||||||
|
checkDirection(Vector3f(0.f, 0.f, 1.f), M_PI_2_F);
|
||||||
|
checkEuler(M_PI_F, 0.f, -M_PI_2_F);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(ControlMathAttitudeMappingTest, AttitudeMappingRandomDirections)
|
||||||
|
{
|
||||||
|
checkDirection(Vector3f(0, .5f, -.5f), 1.f);
|
||||||
|
checkDirection(Vector3f(-2.f, 8.f, .1f), 2.f);
|
||||||
|
checkDirection(Vector3f(-.2f, -5.f, -30.f), 2.f);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(ControlMathTest, ConstrainXYPriorities)
|
TEST(ControlMathTest, ConstrainXYPriorities)
|
||||||
|
|
|
@ -0,0 +1,70 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2020 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 <ControlMath.hpp>
|
||||||
|
#include <AttitudeControl.hpp>
|
||||||
|
#include <px4_defines.h>
|
||||||
|
|
||||||
|
using namespace matrix;
|
||||||
|
|
||||||
|
TEST(PositionControlTest, NavigationYawInfluence)
|
||||||
|
{
|
||||||
|
const Vector3f thrust_setpoint(1.f, 1.f, -.5f);
|
||||||
|
const float yaw_setpoint = 0.f;
|
||||||
|
|
||||||
|
// Generate attitude
|
||||||
|
vehicle_attitude_setpoint_s attitude_setpoint{};
|
||||||
|
ControlMath::thrustToAttitude(thrust_setpoint, yaw_setpoint, attitude_setpoint);
|
||||||
|
|
||||||
|
// Set up attitude control
|
||||||
|
AttitudeControl attitude_control;
|
||||||
|
attitude_control.setProportionalGain(Vector3f(1.f, 1.f, 1.f), 1.f);
|
||||||
|
attitude_control.setRateLimit(Vector3f(100.f, 100.f, 100.f));
|
||||||
|
|
||||||
|
// Execute on attitude
|
||||||
|
Quatf qd(attitude_setpoint.q_d);
|
||||||
|
attitude_control.setAttitudeSetpoint(qd, 0.f);
|
||||||
|
const Vector3f rate_setpoint = attitude_control.update(Quatf());
|
||||||
|
rate_setpoint.print();
|
||||||
|
|
||||||
|
// expect symmetric angular rate command towards thrust direction without any body yaw correction
|
||||||
|
EXPECT_GT(rate_setpoint(0), 0.f);
|
||||||
|
EXPECT_LT(rate_setpoint(1), 0.f);
|
||||||
|
EXPECT_FLOAT_EQ(fabsf(rate_setpoint(0)), fabsf(rate_setpoint(1)));
|
||||||
|
EXPECT_FLOAT_EQ(rate_setpoint(2), 0.f);
|
||||||
|
|
||||||
|
qd.print();
|
||||||
|
Eulerf(qd).print();
|
||||||
|
EXPECT_FLOAT_EQ(2.f * atan2f(qd(3), qd(0)), yaw_setpoint);
|
||||||
|
}
|
Loading…
Reference in New Issue