fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 06:23:34 +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);
src/lib/npfg/npfg.hpp
+2 -2
View File
@@ -240,12 +240,12 @@ public:
* @param[in] loiter_center The position of the center of the loiter circle [m]
* @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
* @param[in] radius Loiter radius [m]
* @param[in] loiter_direction Loiter direction: -1=counter-clockwise, 1=clockwise
* @param[in] loiter_direction_counter_clockwise Specifies loiter direction
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
void navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
float radius, int8_t loiter_direction, const matrix::Vector2f &ground_vel,
float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
/*