fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 06:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

MC auto - Modify yaw_sp_aligned to take mis_yaw_error tolerance.

Browse Source 
Add check for this flag in AutoLine
This commit is contained in:
bresch
2019-08-09 12:04:48 +02:00
committed by Mathieu Bresciani
parent 9e9b2ab9e8
commit de10f1e04d
5 changed files with 14 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+4 -2
View File
@@ -191,10 +191,12 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
float speed_at_target = 0.0f;
const float distance_current_next = Vector2f(&(_target - _next_wp)(0)).length();
const bool waypoint_overlap = Vector2f(&(_target - _prev_wp)(0)).length() < _target_acceptance_radius;
const bool yaw_align_check_pass = (_param_mpc_yaw_mode.get() != 4) || _yaw_sp_aligned;
if (distance_current_next > 0.001f &&
(Vector2f(&(_target - _prev_wp)(0)).length() > _target_acceptance_radius) &&
_param_mpc_yaw_mode.get() != 4) {
!waypoint_overlap &&
yaw_align_check_pass) {
// Max speed between current and next
const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next);
const float alpha = acos(Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() * Vector2f(&(_target - _next_wp)(