34 #include <gtest/gtest.h> 36 #include <px4_defines.h> 40 TEST(PositionControlTest, EmptySetpoint)
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);
63 EXPECT_EQ(
attitude.yaw_sp_move_rate, 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);
79 _position_control.setPositionGains(
Vector3f(1.
f, 1.
f, 1.
f));
81 _position_control.setVelocityLimits(1.
f, 1.
f, 1.
f);
82 _position_control.setThrustLimits(0.1
f, 0.9
f);
83 _position_control.setTiltLimit(1.
f);
84 _position_control.setHoverThrust(.5
f);
86 _contraints.tilt = 1.f;
87 _contraints.speed_xy = NAN;
88 _contraints.speed_up = NAN;
89 _contraints.speed_down = NAN;
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);
105 _position_control.updateConstraints(_contraints);
106 _position_control.updateSetpoint(_input_setpoint);
107 _position_control.generateThrustYawSetpoint(.1
f);
108 _position_control.getLocalPositionSetpoint(_output_setpoint);
109 _position_control.getAttitudeSetpoint(_attitude);
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);
130 EXPECT_LT(body_z(0), 0.
f);
131 EXPECT_LT(body_z(1), 0.
f);
132 EXPECT_GT(body_z(2), 0.
f);
138 _input_setpoint.x = .1f;
139 _input_setpoint.y = .1f;
140 _input_setpoint.z = -.1f;
147 _input_setpoint.vx = .1f;
148 _input_setpoint.vy = .1f;
149 _input_setpoint.vz = -.1f;
156 _input_setpoint.x = 10.f;
157 _input_setpoint.y = 10.f;
158 _input_setpoint.z = -0.f;
163 EXPECT_GT(angle, 0.
f);
164 EXPECT_LE(angle, 1.
f);
166 _contraints.tilt = .5f;
168 body_z =
Quatf(_attitude.q_d).dcm_z();
170 EXPECT_GT(angle, 0.
f);
171 EXPECT_LE(angle, .50001
f);
176 _input_setpoint.x = 10.f;
177 _input_setpoint.y = 10.f;
178 _input_setpoint.z = -10.f;
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);
188 _input_setpoint.x = 10.f;
189 _input_setpoint.y = 10.f;
190 _input_setpoint.z = -10.f;
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);
201 _input_setpoint.vz = .7f;
202 _input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f;
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);
PositionControlBasicTest()
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 ...
Type dot(const MatrixM1 &b) const
Core Position-Control for MC.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
PositionControl _position_control
Vector3< float > Vector3f
TEST_F(PositionControlBasicDirectionTest, PositionControlDirectionP)
Quaternion< float > Quatf
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...
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
TEST(PositionControlTest, EmptySetpoint)