From 1fb4f960cdef6bc47f4fdeb63b6984b836d89e2e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 15 Oct 2020 10:51:12 +0200 Subject: [PATCH] FlightTaskAuto: Lock down yaw when waypoint is reached MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This is done to prevent yawing 180 degrees when switching to hold mode with high velocity and the overshoot causing a 180° yaw turn. The yaw is unlocked on waypoint updates and the yaw setpoint generation had to be put at the end because otherwise the first yaw calculation is with the old waypoint and immediately locks again. --- .../tasks/Auto/FlightTaskAuto.cpp | 52 +++++++++---------- 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp index 0098bb2e57..2c5ff1583a 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp @@ -256,6 +256,26 @@ bool FlightTaskAuto::_evaluateTriplets() _ext_yaw_handler->deactivate(); } + // Calculate the current vehicle state and check if it has updated. + State previous_state = _current_state; + _current_state = _getCurrentState(); + + if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) { + _updateInternalWaypoints(); + _mission_gear = _sub_triplet_setpoint.get().current.landing_gear; + _yaw_lock = false; + } + + if (_param_com_obs_avoid.get() + && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, + _triplet_next_wp, + _sub_triplet_setpoint.get().next.yaw, + _sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : NAN, + _ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type); + _obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt); + } + // set heading if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { _yaw_setpoint = _yaw; @@ -293,25 +313,6 @@ bool FlightTaskAuto::_evaluateTriplets() _yawspeed_setpoint = NAN; } - // Calculate the current vehicle state and check if it has updated. - State previous_state = _current_state; - _current_state = _getCurrentState(); - - if (triplet_update || (_current_state != previous_state) || _current_state == State::offtrack) { - _updateInternalWaypoints(); - _mission_gear = _sub_triplet_setpoint.get().current.landing_gear; - } - - if (_param_com_obs_avoid.get() - && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, - _triplet_next_wp, - _sub_triplet_setpoint.get().next.yaw, - _sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : NAN, - _ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint.get().current.type); - _obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt); - } - return true; } @@ -323,7 +324,7 @@ void FlightTaskAuto::_set_heading_from_mode() switch (_param_mpc_yaw_mode.get()) { case 0: // Heading points towards the current waypoint. - case 4: // Same as 0 but yaw fisrt and then go + case 4: // Same as 0 but yaw first and then go v = Vector2f(_target) - Vector2f(_position); break; @@ -352,14 +353,13 @@ void FlightTaskAuto::_set_heading_from_mode() // 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() > _target_acceptance_radius) { - _compute_heading_from_2D_vector(_yaw_setpoint, v); - _yaw_lock = false; - - } else { - if (!_yaw_lock) { + if (!_yaw_lock) { + if (v.length() < _target_acceptance_radius) { _yaw_setpoint = _yaw; _yaw_lock = true; + + } else { + _compute_heading_from_2D_vector(_yaw_setpoint, v); } }