From da4d6dc6577728ee97eeabcefcc7c31d0c014de6 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 29 Apr 2021 19:19:07 +0200 Subject: [PATCH] L1: increase the max allowed tangential velocity in the opposite direction to 2m/s There is logic in L1 that prevents the vehicle from trying to achieve an impossible loiter entry (e.g. due to wind). That check makes the vehicle track the loiter center if the tangential velocity is in the wrong direction while loitering. After the vehicle flies through the center, it can then turn the other way around to join the loiter. This check is though too sensitive if it purely checks for the wrong direction, and it can end in delayed loiter entry for no reason. This commit increases the threshold to 2m/s of tangential velocity in the wrong direction to trigger the check. Signed-off-by: Silvan Fuhrer --- src/lib/l1/ECL_L1_Pos_Controller.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp index 5174f9d3f1..02f7eae830 100644 --- a/src/lib/l1/ECL_L1_Pos_Controller.cpp +++ b/src/lib/l1/ECL_L1_Pos_Controller.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -276,7 +276,9 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d float tangent_vel = xtrack_vel_center * loiter_direction; /* prevent PD output from turning the wrong way when in circle mode */ - if (tangent_vel < 0.0f && _circle_mode) { + const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction + + if (tangent_vel < -l1_op_tan_vel && _circle_mode) { lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd, 0.0f); }