fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskAuto: lock yaw once within acceptance radius

Browse Source
This commit is contained in:
Dennis Mannhart 2018-08-23 08:25:23 +02:00 committed by ChristophTobler
parent 72f1fca55c
commit 1f1c9fde01
2 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
View File

@ -256,13 +256,20 @@ void FlightTaskAuto::_set_heading_from_mode()
}
if (PX4_ISFINITE(v.length())) {
// We only adjust yaw if vehicle is outside of acceptance radius.
// We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance
// radius, lock yaw to current yaw.
// This prevents excessive yawing.
if (v.length() > NAV_ACC_RAD.get()) {
_compute_heading_from_2D_vector(_yaw_setpoint, v);
_yaw_lock = false;
} else {
if (!_yaw_lock) {
_yaw_setpoint = _yaw;
_yaw_lock = true;
}
} else {
_yaw_lock = false;
_yaw_setpoint = NAN;
}
}

2
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
View File

@ -107,7 +107,7 @@ protected:
private:
matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */
uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
matrix::Vector3f