mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-29 19:04:08 +08:00
b6de83117e
The plot of the attitude setpoint in the log did not show any values because the message timestamp that the position control module sets was overwritten by the PositionControl attitude generation.
180 lines
6.1 KiB
C++
180 lines
6.1 KiB
C++
/****************************************************************************
|
|
*
|
|
* 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 <ControlMath.hpp>
|
|
#include <px4_platform_common/defines.h>
|
|
|
|
using namespace matrix;
|
|
using namespace ControlMath;
|
|
|
|
|
|
TEST(ControlMathTest, ThrottleAttitudeMapping)
|
|
{
|
|
/* expected: zero roll, zero pitch, zero yaw, full thr mag
|
|
* reason: thrust pointing full upward */
|
|
Vector3f thr{0.0f, 0.0f, -1.0f};
|
|
float yaw = 0.0f;
|
|
vehicle_attitude_setpoint_s att{};
|
|
thrustToAttitude(thr, yaw, att);
|
|
EXPECT_EQ(att.roll_body, 0);
|
|
EXPECT_EQ(att.pitch_body, 0);
|
|
EXPECT_EQ(att.yaw_body, 0);
|
|
EXPECT_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_EQ(att.roll_body, 0);
|
|
EXPECT_EQ(att.pitch_body, 0);
|
|
EXPECT_EQ(att.yaw_body, M_PI_2_F);
|
|
EXPECT_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.0f, 0.0f, 1.0f);
|
|
thrustToAttitude(thr, yaw, att);
|
|
EXPECT_NEAR(att.roll_body, -M_PI_F, 1e-4);
|
|
EXPECT_EQ(att.pitch_body, 0);
|
|
EXPECT_EQ(att.yaw_body, M_PI_2_F);
|
|
EXPECT_EQ(att.thrust_body[2], -1.f);
|
|
}
|
|
|
|
TEST(ControlMathTest, ConstrainXYPriorities)
|
|
{
|
|
const float max = 5.0f;
|
|
// v0 already at max
|
|
Vector2f v0(max, 0);
|
|
Vector2f v1(v0(1), -v0(0));
|
|
|
|
Vector2f v_r = constrainXY(v0, v1, max);
|
|
EXPECT_EQ(v_r(0), max);
|
|
EXPECT_EQ(v_r(1), 0);
|
|
|
|
// norm of v1 exceeds max but v0 is zero
|
|
v0.zero();
|
|
v_r = constrainXY(v0, v1, max);
|
|
EXPECT_EQ(v_r(1), -max);
|
|
EXPECT_EQ(v_r(0), 0);
|
|
|
|
v0 = Vector2f(0.5f, 0.5f);
|
|
v1 = Vector2f(0.5f, -0.5f);
|
|
v_r = constrainXY(v0, v1, max);
|
|
const float diff = Vector2f(v_r - (v0 + v1)).length();
|
|
EXPECT_EQ(diff, 0);
|
|
|
|
// v0 and v1 exceed max and are perpendicular
|
|
v0 = Vector2f(4.0f, 0.0f);
|
|
v1 = Vector2f(0.0f, -4.0f);
|
|
v_r = constrainXY(v0, v1, max);
|
|
EXPECT_EQ(v_r(0), v0(0));
|
|
EXPECT_GT(v_r(0), 0);
|
|
const float remaining = sqrtf(max * max - (v0(0) * v0(0)));
|
|
EXPECT_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.0f, 0.0f, 0.0f);
|
|
const Vector3f curr = Vector3f(0.0f, 0.0f, 2.0f);
|
|
Vector3f res;
|
|
bool retval = false;
|
|
|
|
// on line, near, before previous waypoint
|
|
retval = cross_sphere_line(Vector3f(0.0f, 0.0f, -0.5f), 1.0f, 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.0f, 0.0f, 1.0f), 1.0f, 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.0f, 0.0f, 2.5f), 1.0f, 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.0f, 0.5f, -0.5f), 1.0f, prev, curr, res);
|
|
EXPECT_TRUE(retval);
|
|
EXPECT_EQ(res, Vector3f(0.f, 0.f, 0.366025388f));
|
|
|
|
// near, before target waypoint
|
|
retval = cross_sphere_line(Vector3f(0.0f, 0.5f, 1.0f), 1.0f, 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.0f, 0.5f, 2.5f), 1.0f, 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.0f, 2.0f, -0.5f), 1.0f, prev, curr, res);
|
|
EXPECT_FALSE(retval);
|
|
EXPECT_EQ(res, Vector3f());
|
|
|
|
// far, before target waypoint
|
|
retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.0f, 2.0f, 1.0f), 1.0f, 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.0f, 2.0f, 2.5f), 1.0f, prev, curr, res);
|
|
EXPECT_FALSE(retval);
|
|
EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f));
|
|
}
|