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

position_setpoint: replaced loiter_direction integer by boolean (#20317)

Browse Source 
* position_setpoint: replaced loiter_direction integer by boolean (loiter_direction_counter_clockwise)

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Roman Bapst
2022-09-29 11:06:10 +02:00
committed by GitHub
parent 5d39fdba6d
commit 20457c5e2e
9 changed files with 36 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/npfg/npfg.cpp
+6 -4
View File
@@ -574,12 +574,14 @@ void NPFG::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoin
} // navigateWaypoints
void NPFG::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos,
float radius, int8_t loiter_direction, const Vector2f &ground_vel, const Vector2f &wind_vel)
float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
path_type_loiter_ = true;
radius = math::max(radius, MIN_RADIUS);
const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
Vector2f vector_center_to_vehicle = vehicle_pos - loiter_center;
const float dist_to_center = vector_center_to_vehicle.norm();
@@ -605,12 +607,12 @@ void NPFG::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle
}
// 90 deg clockwise rotation * loiter direction
unit_path_tangent_ = float(loiter_direction) * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
unit_path_tangent_ = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
// positive in direction of path normal
signed_track_error_ = -loiter_direction * (dist_to_center - radius);
signed_track_error_ = -loiter_direction_multiplier * (dist_to_center - radius);
float path_curvature = float(loiter_direction) / radius;
float path_curvature = loiter_direction_multiplier / radius;
guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, path_curvature);