PX4 Firmware
PX4 Autopilot Software http://px4.io
PositionControlTest.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #include <gtest/gtest.h>
35 #include <PositionControl.hpp>
36 #include <px4_defines.h>
37 
38 using namespace matrix;
39 
40 TEST(PositionControlTest, EmptySetpoint)
41 {
42  PositionControl position_control;
43 
44  vehicle_local_position_setpoint_s output_setpoint{};
45  position_control.getLocalPositionSetpoint(output_setpoint);
46  EXPECT_EQ(output_setpoint.x, 0.f);
47  EXPECT_EQ(output_setpoint.y, 0.f);
48  EXPECT_EQ(output_setpoint.z, 0.f);
49  EXPECT_EQ(output_setpoint.yaw, 0.f);
50  EXPECT_EQ(output_setpoint.yawspeed, 0.f);
51  EXPECT_EQ(output_setpoint.vx, 0.f);
52  EXPECT_EQ(output_setpoint.vy, 0.f);
53  EXPECT_EQ(output_setpoint.vz, 0.f);
54  EXPECT_EQ(Vector3f(output_setpoint.acceleration), Vector3f(0.f, 0.f, 0.f));
55  EXPECT_EQ(Vector3f(output_setpoint.jerk), Vector3f(0.f, 0.f, 0.f));
56  EXPECT_EQ(Vector3f(output_setpoint.thrust), Vector3f(0, 0, 0));
57 
59  position_control.getAttitudeSetpoint(attitude);
60  EXPECT_EQ(attitude.roll_body, 0.f);
61  EXPECT_EQ(attitude.pitch_body, 0.f);
62  EXPECT_EQ(attitude.yaw_body, 0.f);
63  EXPECT_EQ(attitude.yaw_sp_move_rate, 0.f);
64  EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f));
65  //EXPECT_EQ(attitude.q_d_valid, false); // TODO should not be true when there was no control
66  EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f));
67  EXPECT_EQ(attitude.roll_reset_integral, false);
68  EXPECT_EQ(attitude.pitch_reset_integral, false);
69  EXPECT_EQ(attitude.yaw_reset_integral, false);
70  EXPECT_EQ(attitude.fw_control_yaw, false);
71  EXPECT_EQ(attitude.apply_flaps, 0.f);//vehicle_attitude_setpoint_s::FLAPS_OFF); // TODO why no reference?
72 }
73 
74 class PositionControlBasicTest : public ::testing::Test
75 {
76 public:
78  {
79  _position_control.setPositionGains(Vector3f(1.f, 1.f, 1.f));
80  _position_control.setVelocityGains(Vector3f(1.f, 1.f, 1.f), Vector3f(1.f, 1.f, 1.f), Vector3f(1.f, 1.f, 1.f));
81  _position_control.setVelocityLimits(1.f, 1.f, 1.f);
82  _position_control.setThrustLimits(0.1f, 0.9f);
83  _position_control.setTiltLimit(1.f);
84  _position_control.setHoverThrust(.5f);
85 
86  _contraints.tilt = 1.f;
87  _contraints.speed_xy = NAN;
88  _contraints.speed_up = NAN;
89  _contraints.speed_down = NAN;
90 
91  _input_setpoint.x = NAN;
92  _input_setpoint.y = NAN;
93  _input_setpoint.z = NAN;
94  _input_setpoint.yaw = NAN;
95  _input_setpoint.yawspeed = NAN;
96  _input_setpoint.vx = NAN;
97  _input_setpoint.vy = NAN;
98  _input_setpoint.vz = NAN;
99  Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.acceleration);
100  Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.thrust);
101  }
102 
104  {
105  _position_control.updateConstraints(_contraints);
106  _position_control.updateSetpoint(_input_setpoint);
107  _position_control.generateThrustYawSetpoint(.1f);
108  _position_control.getLocalPositionSetpoint(_output_setpoint);
109  _position_control.getAttitudeSetpoint(_attitude);
110  }
111 
113  vehicle_constraints_s _contraints{};
117 };
118 
120 {
121 public:
123  {
124  Vector3f thrust(_output_setpoint.thrust);
125  EXPECT_GT(thrust(0), 0.f);
126  EXPECT_GT(thrust(1), 0.f);
127  EXPECT_LT(thrust(2), 0.f);
128 
129  Vector3f body_z = Quatf(_attitude.q_d).dcm_z();
130  EXPECT_LT(body_z(0), 0.f);
131  EXPECT_LT(body_z(1), 0.f);
132  EXPECT_GT(body_z(2), 0.f);
133  }
134 };
135 
136 TEST_F(PositionControlBasicDirectionTest, PositionControlDirectionP)
137 {
138  _input_setpoint.x = .1f;
139  _input_setpoint.y = .1f;
140  _input_setpoint.z = -.1f;
141  runController();
142  checkDirection();
143 }
144 
145 TEST_F(PositionControlBasicDirectionTest, VelocityControlDirection)
146 {
147  _input_setpoint.vx = .1f;
148  _input_setpoint.vy = .1f;
149  _input_setpoint.vz = -.1f;
150  runController();
151  checkDirection();
152 }
153 
154 TEST_F(PositionControlBasicTest, PositionControlMaxTilt)
155 {
156  _input_setpoint.x = 10.f;
157  _input_setpoint.y = 10.f;
158  _input_setpoint.z = -0.f;
159 
160  runController();
161  Vector3f body_z = Quatf(_attitude.q_d).dcm_z();
162  float angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
163  EXPECT_GT(angle, 0.f);
164  EXPECT_LE(angle, 1.f);
165 
166  _contraints.tilt = .5f;
167  runController();
168  body_z = Quatf(_attitude.q_d).dcm_z();
169  angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
170  EXPECT_GT(angle, 0.f);
171  EXPECT_LE(angle, .50001f);
172 }
173 
174 TEST_F(PositionControlBasicTest, PositionControlMaxVelocity)
175 {
176  _input_setpoint.x = 10.f;
177  _input_setpoint.y = 10.f;
178  _input_setpoint.z = -10.f;
179 
180  runController();
181  Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy);
182  EXPECT_LE(velocity_xy.norm(), 1.f);
183  EXPECT_LE(abs(_output_setpoint.vz), 1.f);
184 }
185 
186 TEST_F(PositionControlBasicTest, PositionControlThrustLimit)
187 {
188  _input_setpoint.x = 10.f;
189  _input_setpoint.y = 10.f;
190  _input_setpoint.z = -10.f;
191 
192  runController();
193  EXPECT_EQ(_attitude.thrust_body[0], 0.f);
194  EXPECT_EQ(_attitude.thrust_body[1], 0.f);
195  EXPECT_LT(_attitude.thrust_body[2], -.1f);
196  EXPECT_GE(_attitude.thrust_body[2], -0.9f);
197 }
198 
199 TEST_F(PositionControlBasicTest, PositionControlFailsafeInput)
200 {
201  _input_setpoint.vz = .7f;
202  _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f;
203 
204  runController();
205  EXPECT_EQ(_attitude.thrust_body[0], 0.f);
206  EXPECT_EQ(_attitude.thrust_body[1], 0.f);
207  EXPECT_LT(_output_setpoint.thrust[2], -.1f);
208  EXPECT_GT(_output_setpoint.thrust[2], -.5f);
209  EXPECT_GT(_attitude.thrust_body[2], -.5f);
210  EXPECT_LE(_attitude.thrust_body[2], -.1f);
211 }
A cascaded position controller for position/velocity control only.
void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
Get the controllers output attitude setpoint This attitude setpoint was generated from the resulting ...
Quaternion class.
Definition: Dcm.hpp:24
Type dot(const MatrixM1 &b) const
Definition: Vector.hpp:55
Core Position-Control for MC.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Vector3< float > Vector3f
Definition: Vector3.hpp:136
TEST_F(PositionControlBasicDirectionTest, PositionControlDirectionP)
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
void getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const
Get the controllers output local position setpoint These setpoints are the ones which were executed o...
Type norm() const
Definition: Vector.hpp:73
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Definition: Dual.hpp:196
TEST(PositionControlTest, EmptySetpoint)