diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 054502e131..4ad54f5f88 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -160,6 +160,7 @@ bool FlightTaskAuto::_evaluateTriplets() } else { _triplet_target = tmp_target; + _target_acceptance_radius = _sub_triplet_setpoint->get().current.acceptance_radius; if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) { // Horizontal target is not finite. @@ -268,7 +269,7 @@ 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() > NAV_ACC_RAD.get()) { + if (v.length() > _target_acceptance_radius) { _compute_heading_from_2D_vector(_yaw_setpoint, v); _yaw_lock = false; @@ -331,7 +332,7 @@ void FlightTaskAuto::_checkAvoidanceProgress() const float pos_to_target_z = fabsf(_triplet_target(2) - _position(2)); - if (pos_to_target.length() < NAV_ACC_RAD.get() && pos_to_target_z > NAV_MC_ALT_RAD.get()) { + if (pos_to_target.length() < _target_acceptance_radius && pos_to_target_z > NAV_MC_ALT_RAD.get()) { // vehicle above or below the target waypoint pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f; } @@ -479,7 +480,7 @@ void FlightTaskAuto::_updateInternalWaypoints() // angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 if (Vector2f(_target - _next_wp).length() > 0.001f && - (Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) { + (Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) { angle = Vector2f(_target - _prev_wp).unit_or_zero() * Vector2f(_target - _next_wp).unit_or_zero() @@ -492,6 +493,7 @@ void FlightTaskAuto::_updateInternalWaypoints() case State::previous_infront: { _next_wp = _triplet_target; _target = _triplet_prev_wp; + _target_acceptance_radius = _sub_triplet_setpoint->get().previous.acceptance_radius; _prev_wp = _position; float angle = 2.0f; @@ -500,7 +502,7 @@ void FlightTaskAuto::_updateInternalWaypoints() // angle = cos(x) + 1.0 // angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 if (Vector2f(_target - _next_wp).length() > 0.001f && - (Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) { + (Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) { angle = Vector2f(_target - _prev_wp).unit_or_zero() * Vector2f(_target - _next_wp).unit_or_zero() @@ -521,7 +523,7 @@ void FlightTaskAuto::_updateInternalWaypoints() // angle = cos(x) + 1.0 // angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 if (Vector2f(_target - _next_wp).length() > 0.001f && - (Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) { + (Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) { angle = Vector2f(_target - _prev_wp).unit_or_zero() * Vector2f(_target - _next_wp).unit_or_zero() @@ -542,7 +544,7 @@ void FlightTaskAuto::_updateInternalWaypoints() // angle = cos(x) + 1.0 // angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 if (Vector2f(_target - _next_wp).length() > 0.001f && - (Vector2f(_target - _prev_wp).length() > NAV_ACC_RAD.get())) { + (Vector2f(_target - _prev_wp).length() > _target_acceptance_radius)) { angle = Vector2f(_target - _prev_wp).unit_or_zero() diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index 5293eb0a7a..f90a8a3f83 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -104,11 +104,11 @@ protected: State _current_state{State::none}; float _speed_at_target = 0.0f; /**< Desired velocity at target. */ + float _target_acceptance_radius = 0.0f; /**< Acceptances radius of the target */ DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, (ParamFloat) MPC_XY_CRUISE, (ParamFloat) MPC_CRUISE_90, // speed at corner when angle is 90 degrees move to line - (ParamFloat) NAV_ACC_RAD, // acceptance radius at which waypoints are updated move to line (ParamFloat) NAV_MC_ALT_RAD, //vertical acceptance radius at which waypoints are updated (ParamInt) MPC_YAW_MODE, // defines how heading is executed, (ParamInt) MPC_OBS_AVOID // obstacle avoidance active diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp index a5a0400c87..4d2bdf9d72 100644 --- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp @@ -63,10 +63,10 @@ void FlightTaskAutoLine::_generateHeadingAlongTrack() void FlightTaskAutoLine::_generateXYsetpoints() { Vector2f pos_sp_to_dest(_target - _position_setpoint); - const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < NAV_ACC_RAD.get(); + const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _target_acceptance_radius; - if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < NAV_ACC_RAD.get()) || - (!has_reached_altitude && pos_sp_to_dest.length() < NAV_ACC_RAD.get())) { + if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _target_acceptance_radius) || + (!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) { // Vehicle reached target in xy and no passing required. Lock position */ _position_setpoint(0) = _target(0); @@ -96,9 +96,9 @@ void FlightTaskAutoLine::_generateXYsetpoints() } // Compute maximum speed at target threshold */ - if (threshold_max > NAV_ACC_RAD.get()) { - float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - NAV_ACC_RAD.get()); - speed_threshold = m * (target_threshold - NAV_ACC_RAD.get()) + _speed_at_target; // speed at transition + if (threshold_max > _target_acceptance_radius) { + float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _target_acceptance_radius); + speed_threshold = m * (target_threshold - _target_acceptance_radius) + _speed_at_target; // speed at transition } // Either accelerate or decelerate @@ -111,7 +111,7 @@ void FlightTaskAutoLine::_generateXYsetpoints() _speed_at_target = 0.0f; } - float acceptance_radius = NAV_ACC_RAD.get(); + float acceptance_radius = _target_acceptance_radius; if (_speed_at_target < 0.01f) { // If vehicle wants to stop at the target, then set acceptance radius to zero as well.