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

Fix bezier scaling

Browse Source
This commit is contained in:
Julian Kent
2020-04-06 09:02:12 +02:00
committed by Julian Kent
parent 025799248c
commit 9404b19906
2 changed files with 17 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/avoidance/ObstacleAvoidance.cpp
+3 -3
View File
@@ -153,9 +153,9 @@ void ObstacleAvoidance::_generateBezierSetpoints(matrix::Vector3f &position, mat
bezier::calculateBezierPosVel(bezier_points, bezier_order, T, position, velocity) &&
bezier::calculateBezierYaw(bezier_yaws, bezier_order, T, yaw, yaw_velocity)
) {
// translate velocities into real velocities
yaw_velocity *= duration_s;
velocity *= duration_s;
// translate bezier velocities T [0;1] into real velocities m/s
yaw_velocity /= duration_s;
velocity /= duration_s;
} else {
PX4_WARN("Obstacle Avoidance system failed, bad trajectory");
src/lib/avoidance/ObstacleAvoidanceTest.cpp
+14 -7
View File
@@ -109,12 +109,17 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier)
vehicle_trajectory_bezier_s message {};
message.timestamp = hrt_absolute_time();
message.bezier_order = 1;
message.bezier_order = 2;
message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0] = 2.6f;
message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1] = 2.4f;
message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2] = 2.7f;
message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw = 0.23f;
message.control_points[vehicle_trajectory_bezier_s::POINT_0].delta = 0.5f;
message.control_points[vehicle_trajectory_bezier_s::POINT_0].delta = NAN;
message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[0] = 2.6f;
message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[1] = 2.4f;
message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[2] = 3.7f;
message.control_points[vehicle_trajectory_bezier_s::POINT_1].yaw = 0.23f;
message.control_points[vehicle_trajectory_bezier_s::POINT_1].delta = 0.5f;
// GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message
uORB::Publication<vehicle_trajectory_bezier_s> vehicle_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
@@ -129,11 +134,13 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier)
oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);
// THEN: the setpoints should be injected
EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0], pos_sp(0));
EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1], pos_sp(1));
EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2], pos_sp(2));
EXPECT_FLOAT_EQ(vel_sp.norm(), 0);
EXPECT_FLOAT_EQ(message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw, yaw_sp);
EXPECT_FLOAT_EQ(2.6f, pos_sp(0));
EXPECT_FLOAT_EQ(2.4f, pos_sp(1));
EXPECT_LT(2.7f, pos_sp(2));
EXPECT_GT(2.8f, pos_sp(2)); // probably only a tiny bit above 2.7, but let's not have flakey tests
EXPECT_FLOAT_EQ(vel_sp.xy().norm(), 0);
EXPECT_FLOAT_EQ(vel_sp(2), (3.7f - 2.7f) / 0.5f);
EXPECT_FLOAT_EQ(0.23, yaw_sp);
EXPECT_FLOAT_EQ(yaw_speed_sp, 0);
}