fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 19:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix confusion between trajectory_setpoint and vehicle_local_postion_setpoint

Browse Source
This commit is contained in:
Matthias Grob
2022-10-17 16:18:00 -04:00
committed by Daniel Agar
parent 7c237fca74
commit 75c63aee2a
13 changed files with 91 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+5 -3
View File
@@ -44,6 +44,8 @@
using namespace matrix;
const trajectory_setpoint_s PositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
{
_gain_vel_p = P;
@@ -93,10 +95,10 @@ void PositionControl::setState(const PositionControlStates &states)
_vel_dot = states.acceleration;
}
void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
void PositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint)
{
_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
_pos_sp = Vector3f(setpoint.position);
_vel_sp = Vector3f(setpoint.velocity);
_acc_sp = Vector3f(setpoint.acceleration);
_yaw_sp = setpoint.yaw;
_yawspeed_sp = setpoint.yawspeed;
src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+8 -2
View File
@@ -41,6 +41,7 @@
#include <lib/mathlib/mathlib.h>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -141,9 +142,9 @@ public:
/**
* Pass the desired setpoints
* Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint.
* @param setpoint a vehicle_local_position_setpoint_s structure
* @param setpoint setpoints including feed-forwards to execute in update()
*/
void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint);
void setInputSetpoint(const trajectory_setpoint_s &setpoint);
/**
* Apply P-position and PID-velocity controller that updates the member
@@ -178,6 +179,11 @@ public:
*/
void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
/**
* All setpoints are set to NAN (uncontrolled). Timestampt zero.
*/
static const trajectory_setpoint_s empty_trajectory_setpoint;
private:
bool _inputValid();
src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
+31 -66
View File
@@ -79,22 +79,6 @@ public:
_position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN);
_position_control.setTiltLimit(1.f);
_position_control.setHoverThrust(.5f);
resetInputSetpoint();
}
void resetInputSetpoint()
{
_input_setpoint.x = NAN;
_input_setpoint.y = NAN;
_input_setpoint.z = NAN;
_input_setpoint.yaw = NAN;
_input_setpoint.yawspeed = NAN;
_input_setpoint.vx = NAN;
_input_setpoint.vy = NAN;
_input_setpoint.vz = NAN;
Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.acceleration);
Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.thrust);
}
bool runController()
@@ -107,7 +91,7 @@ public:
}
PositionControl _position_control;
vehicle_local_position_setpoint_s _input_setpoint{};
trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint};
vehicle_local_position_setpoint_s _output_setpoint{};
vehicle_attitude_setpoint_s _attitude{};
@@ -134,27 +118,21 @@ public:
TEST_F(PositionControlBasicDirectionTest, PositionDirection)
{
_input_setpoint.x = .1f;
_input_setpoint.y = .1f;
_input_setpoint.z = -.1f;
Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.position);
EXPECT_TRUE(runController());
checkDirection();
}
TEST_F(PositionControlBasicDirectionTest, VelocityDirection)
{
_input_setpoint.vx = .1f;
_input_setpoint.vy = .1f;
_input_setpoint.vz = -.1f;
Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.velocity);
EXPECT_TRUE(runController());
checkDirection();
}
TEST_F(PositionControlBasicTest, TiltLimit)
{
_input_setpoint.x = 10.f;
_input_setpoint.y = 10.f;
_input_setpoint.z = -0.f;
Vector3f(10.f, 10.f, 0.f).copyTo(_input_setpoint.position);
EXPECT_TRUE(runController());
Vector3f body_z = Quatf(_attitude.q_d).dcm_z();
@@ -174,9 +152,7 @@ TEST_F(PositionControlBasicTest, TiltLimit)
TEST_F(PositionControlBasicTest, VelocityLimit)
{
_input_setpoint.x = 10.f;
_input_setpoint.y = 10.f;
_input_setpoint.z = -10.f;
Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position);
EXPECT_TRUE(runController());
Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy);
@@ -187,9 +163,7 @@ TEST_F(PositionControlBasicTest, VelocityLimit)
TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
{
// Given a setpoint that drives the controller into vertical and horizontal saturation
_input_setpoint.x = 10.f;
_input_setpoint.y = 10.f;
_input_setpoint.z = -10.f;
Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position);
// When you run it for one iteration
runController();
@@ -218,9 +192,7 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
{
_input_setpoint.x = 10.f;
_input_setpoint.y = 0.f;
_input_setpoint.z = 10.f;
Vector3f(10.f, 0.f, 10.f).copyTo(_input_setpoint.position);
runController();
Vector3f thrust(_output_setpoint.thrust);
@@ -234,9 +206,8 @@ TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
TEST_F(PositionControlBasicTest, FailsafeInput)
{
_input_setpoint.vz = .1f;
_input_setpoint.thrust[0] = _input_setpoint.thrust[1] = 0.f;
_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);
@@ -255,14 +226,12 @@ TEST_F(PositionControlBasicTest, IdleThrustInput)
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);
EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); // minimum thrust
}
TEST_F(PositionControlBasicTest, InputCombinationsPosition)
{
_input_setpoint.x = .1f;
_input_setpoint.y = .2f;
_input_setpoint.z = .3f;
Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position);
EXPECT_TRUE(runController());
EXPECT_FLOAT_EQ(_output_setpoint.x, .1f);
@@ -278,13 +247,13 @@ TEST_F(PositionControlBasicTest, InputCombinationsPosition)
TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
{
_input_setpoint.vx = .1f;
_input_setpoint.vy = .2f;
_input_setpoint.z = .3f; // altitude
_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_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);
@@ -297,9 +266,9 @@ TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
TEST_F(PositionControlBasicTest, SetpointValiditySimple)
{
EXPECT_FALSE(runController());
_input_setpoint.x = .1f;
_input_setpoint.position[0] = .1f;
EXPECT_FALSE(runController());
_input_setpoint.y = .2f;
_input_setpoint.position[1] = .2f;
EXPECT_FALSE(runController());
_input_setpoint.acceleration[2] = .3f;
EXPECT_TRUE(runController());
@@ -308,13 +277,13 @@ TEST_F(PositionControlBasicTest, SetpointValiditySimple)
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.x, &_input_setpoint.vx, &_input_setpoint.acceleration[0],
&_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.acceleration[1],
&_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.acceleration[2]
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++) {
resetInputSetpoint();
_input_setpoint = PositionControl::empty_trajectory_setpoint;
for (int j = 0; j < 9; j++) {
if (combination & (1 << j)) {
@@ -333,8 +302,10 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl
<< "input" << std::endl
<< "position " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl
<< "velocity " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << 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
@@ -347,9 +318,7 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
TEST_F(PositionControlBasicTest, InvalidState)
{
_input_setpoint.x = .1f;
_input_setpoint.y = .2f;
_input_setpoint.z = .3f;
Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position);
PositionControlStates states{};
states.position(0) = NAN;
@@ -377,9 +346,7 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust)
const float hover_thrust = 0.6f;
_position_control.setHoverThrust(hover_thrust);
_input_setpoint.vx = 0.f;
_input_setpoint.vy = 0.f;
_input_setpoint.vz = -0.f;
Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity);
// WHEN: we run the controller
EXPECT_TRUE(runController());
@@ -400,16 +367,14 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust)
TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint)
{
// GIVEN: the controller was ran with an invalid setpoint containing some valid values
_input_setpoint.x = .1f;
_input_setpoint.y = .2f;
_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
resetInputSetpoint();
_input_setpoint.vx = 0.f;
_input_setpoint.vy = 0.f;
_input_setpoint.vz = 0.f;
_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