fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 23:13: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/l1/ECL_L1_Pos_Controller.cpp
+10 -5
View File
@@ -213,10 +213,12 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
void
ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
int8_t loiter_direction, const Vector2f &ground_speed_vector)
const bool loiter_direction_counter_clockwise, const Vector2f &ground_speed_vector)
{
_has_guidance_updated = true;
const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
/* the complete guidance logic in this section was proposed by [2] */
/* calculate the gains for the PD loop (circle tracking) */
@@ -274,7 +276,7 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
/* calculate velocity on circle / along tangent */
float tangent_vel = xtrack_vel_center * loiter_direction;
float tangent_vel = xtrack_vel_center * loiter_direction_multiplier;
/* prevent PD output from turning the wrong way when in circle mode */
const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction
@@ -288,7 +290,8 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
(radius + xtrack_err_circle));
/* add PD control on circle and centripetal acceleration for total circle command */
float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal);
float lateral_accel_sp_circle = loiter_direction_multiplier * (lateral_accel_sp_circle_pd +
lateral_accel_sp_circle_centripetal);
/*
* Switch between circle (loiter) and capture (towards waypoint center) mode when
@@ -296,8 +299,10 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
*/
// XXX check switch over
if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
if ((lateral_accel_sp_center < lateral_accel_sp_circle && !loiter_direction_counter_clockwise
&& xtrack_err_circle > 0.0f)
||
(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction_counter_clockwise && xtrack_err_circle > 0.0f)) {
_lateral_accel = lateral_accel_sp_center;
_circle_mode = false;
/* angle between requested and current velocity vector */
src/lib/l1/ECL_L1_Pos_Controller.hpp
+1 -1
View File
@@ -154,7 +154,7 @@ public:
* @return sets _lateral_accel setpoint
*/
void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector);
const bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_speed_vector);
/**
* Navigate on a fixed bearing.