dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FW L1 controller: also handle negative loiter direction case 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2021-10-20 09:45:22 +02:00
parent f61491995b
commit 3c03d48876
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/lib/l1/ECL_L1_Pos_Controller.cpp
View File

@ -307,8 +307,9 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
_circle_mode = true;
_bearing_error = 0.0f;
// once loitering, the bearing is tangentially to the loiter,
// so subtract PI/2 from the bearing from current position to loiter center
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)) - M_PI_2_F;
// so subtract/add PI/2 from the bearing from current position to loiter center depending on loiter direction
const float center_to_tangent = M_PI_2_F * loiter_direction * -1.f;
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)) + center_to_tangent;
}
update_roll_setpoint();