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

FixedwingPositionControl: reuse line() and waypoint() methods in navigateWaypoints() method

Browse Source
This commit is contained in:
Thomas Stastny
2023-09-08 16:08:38 +02:00
parent ad9e3d72d9
commit e71804d976
1 changed files with 12 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+12 -19
View File
@@ -2819,40 +2819,33 @@ void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint,
const Vector2f start_waypoint_to_end_waypoint = end_waypoint - start_waypoint; const Vector2f start_waypoint_to_end_waypoint = end_waypoint - start_waypoint;
const Vector2f start_waypoint_to_vehicle = vehicle_pos - start_waypoint; const Vector2f start_waypoint_to_vehicle = vehicle_pos - start_waypoint;
const Vector2f end_waypoint_to_vehicle = vehicle_pos - end_waypoint; const Vector2f end_waypoint_to_vehicle = vehicle_pos - end_waypoint;
Vector2f unit_path_tangent{1.f, 0.f};
if (start_waypoint_to_end_waypoint.norm() < FLT_EPSILON) { if (start_waypoint_to_end_waypoint.norm() < FLT_EPSILON) {
// degenerate case: the waypoints are on top of each other, this should only happen when someone uses this // degenerate case: the waypoints are on top of each other, this should only happen when someone uses this
// method incorrectly. just a safe guard, call the singular waypoint navigation method. // method incorrectly. just as a safe guard, call the singular waypoint navigation method.
navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel);
return; return;
}
} else if ((start_waypoint_to_end_waypoint.dot(start_waypoint_to_vehicle) < -FLT_EPSILON) if ((start_waypoint_to_end_waypoint.dot(start_waypoint_to_vehicle) < -FLT_EPSILON)
&& (start_waypoint_to_vehicle.norm() > _npfg.switchDistance(500.0f))) { && (start_waypoint_to_vehicle.norm() > _npfg.switchDistance(500.0f))) {
// we are in front of the start waypoint, fly directly to it until we are within switch distance // we are in front of the start waypoint, fly directly to it until we are within switch distance
unit_path_tangent = -start_waypoint_to_vehicle.normalized(); navigateWaypoint(start_waypoint, vehicle_pos, ground_vel, wind_vel);
_closest_point_on_path = start_waypoint; return;
}
} else if (start_waypoint_to_end_waypoint.dot(end_waypoint_to_vehicle) > FLT_EPSILON) { if (start_waypoint_to_end_waypoint.dot(end_waypoint_to_vehicle) > FLT_EPSILON) {
// we are beyond the end waypoint, fly back to it // we are beyond the end waypoint, fly back to it
// NOTE: this logic ideally never gets executed, as a waypoint switch should happen before passing the // NOTE: this logic ideally never gets executed, as a waypoint switch should happen before passing the
// end waypoint. however this included here as a safety precaution if any navigator (module) switch condition // end waypoint. however this included here as a safety precaution if any navigator (module) switch condition
// is missed for any reason. in the future this logic should all be handled in one place in a dedicated // is missed for any reason. in the future this logic should all be handled in one place in a dedicated
// flight mode state machine. // flight mode state machine.
unit_path_tangent = -end_waypoint_to_vehicle.normalized(); navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel);
_closest_point_on_path = end_waypoint; return;
} else {
// follow the line segment between the start and end waypoints
unit_path_tangent = start_waypoint_to_end_waypoint.normalized();
_closest_point_on_path = start_waypoint + start_waypoint_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
} }
const float path_curvature = 0.f; // follow the line segment between the start and end waypoints
_npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); navigateLine(start_waypoint, end_waypoint, vehicle_pos, ground_vel, wind_vel);
// for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined.
_target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0));
} }
void FixedwingPositionControl::navigateWaypoint(const Vector2f &waypoint_pos, const Vector2f &vehicle_pos, void FixedwingPositionControl::navigateWaypoint(const Vector2f &waypoint_pos, const Vector2f &vehicle_pos,