diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 4ad54f5f88..37c1f6f7ea 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -462,102 +462,34 @@ void FlightTaskAuto::_updateInternalWaypoints() // 1. The vehicle already passed the target -> go straight to target // 2. The vehicle is more than cruise speed in front of previous waypoint -> go straight to previous waypoint // 3. The vehicle is more than cruise speed from track -> go straight to closest point on track - // - // If a new target is available, then the speed at the target is computed from the angle previous-target-next. - switch (_current_state) { - - case State::target_behind: { - _target = _triplet_target; - _prev_wp = _position; - _next_wp = _triplet_next_wp; - //_current_state = State::target_behind; - - float angle = 2.0f; - _speed_at_target = 0.0f; - - // 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() > _target_acceptance_radius)) { - - angle = Vector2f(_target - _prev_wp).unit_or_zero() - * Vector2f(_target - _next_wp).unit_or_zero() - + 1.0f; - _speed_at_target = _getVelocityFromAngle(angle); - } - } + case State::target_behind: + _target = _triplet_target; + _prev_wp = _position; + _next_wp = _triplet_next_wp; break; - 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; - _speed_at_target = 0.0f; - - // 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() > _target_acceptance_radius)) { - - angle = Vector2f(_target - _prev_wp).unit_or_zero() - * Vector2f(_target - _next_wp).unit_or_zero() - + 1.0f; - _speed_at_target = _getVelocityFromAngle(angle); - } - } + case State::previous_infront: + _next_wp = _triplet_target; + _target = _triplet_prev_wp; + _prev_wp = _position; break; - case State::offtrack: { - _next_wp = _triplet_target; - _target = Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2)); - _prev_wp = _position; - - float angle = 2.0f; - _speed_at_target = 0.0f; - - // 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() > _target_acceptance_radius)) { - - angle = Vector2f(_target - _prev_wp).unit_or_zero() - * Vector2f(_target - _next_wp).unit_or_zero() - + 1.0f; - _speed_at_target = _getVelocityFromAngle(angle); - } - } + case State::offtrack: + _next_wp = _triplet_target; + _target = matrix::Vector3f(_closest_pt(0), _closest_pt(1), _triplet_target(2)); + _prev_wp = _position; break; - case State::none: { - _target = _triplet_target; - _prev_wp = _triplet_prev_wp; - _next_wp = _triplet_next_wp; - - float angle = 2.0f; - _speed_at_target = 0.0f; - - // 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() > _target_acceptance_radius)) { - - angle = - Vector2f(_target - _prev_wp).unit_or_zero() - * Vector2f(_target - _next_wp).unit_or_zero() - + 1.0f; - _speed_at_target = _getVelocityFromAngle(angle); - } - - break; - } + case State::none: + _target = _triplet_target; + _prev_wp = _triplet_prev_wp; + _next_wp = _triplet_next_wp; + break; default: break; + } } @@ -575,63 +507,4 @@ bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v) // heading unknown and therefore do not change heading return false; -} - - -float FlightTaskAuto::_getVelocityFromAngle(const float angle) -{ - // minimum cruise speed when passing waypoint - float min_cruise_speed = 0.0f; - - // make sure that cruise speed is larger than minimum - if ((_mc_cruise_speed - min_cruise_speed) < SIGMA_NORM) { - return _mc_cruise_speed; - } - - // Middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle of 90degrees. - // It needs to be always larger than minimum cruise speed. - float middle_cruise_speed = MPC_CRUISE_90.get(); - - if ((middle_cruise_speed - min_cruise_speed) < SIGMA_NORM) { - middle_cruise_speed = min_cruise_speed + SIGMA_NORM; - } - - if ((_mc_cruise_speed - middle_cruise_speed) < SIGMA_NORM) { - middle_cruise_speed = (_mc_cruise_speed + min_cruise_speed) * 0.5f; - } - - // If middle cruise speed is exactly in the middle, then compute speed linearly. - bool use_linear_approach = false; - - if (((_mc_cruise_speed + min_cruise_speed) * 0.5f) - middle_cruise_speed < SIGMA_NORM) { - use_linear_approach = true; - } - - // compute speed sp at target - float speed_close; - - if (use_linear_approach) { - - // velocity close to target adjusted to angle: - // vel_close = m*x+q - float slope = -(_mc_cruise_speed - min_cruise_speed) / 2.0f; - speed_close = slope * angle + _mc_cruise_speed; - - } else { - - // Speed close to target adjusted to angle x. - // speed_close = a *b ^x + c; where at angle x = 0 -> speed_close = cruise; angle x = 1 -> speed_close = middle_cruise_speed (this means that at 90degrees - // the velocity at target is middle_cruise_speed); - // angle x = 2 -> speed_close = min_cruising_speed - - // from maximum cruise speed, minimum cruise speed and middle cruise speed compute constants a, b and c - float a = -((middle_cruise_speed - _mc_cruise_speed) * (middle_cruise_speed - _mc_cruise_speed)) - / (2.0f * middle_cruise_speed - _mc_cruise_speed - min_cruise_speed); - float c = _mc_cruise_speed - a; - float b = (middle_cruise_speed - c) / a; - speed_close = a * powf(b, angle) + c; - } - - // speed_close needs to be in between max and min - return math::constrain(speed_close, min_cruise_speed, _mc_cruise_speed); -} +} \ No newline at end of file diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index 7c7705804d..825e7d2aff 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -102,8 +102,6 @@ protected: uORB::Subscription *_sub_home_position{nullptr}; 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, @@ -140,7 +138,6 @@ private: bool _evaluateTriplets(); /**< Checks and sets triplets. */ bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */ bool _evaluateGlobalReference(); /**< Check is global reference is available. */ - float _getVelocityFromAngle(const float angle); /**< Computes the speed at target depending on angle. */ State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */ void _set_heading_from_mode(); /**< @see MPC_YAW_MODE */ void _checkAvoidanceProgress(); diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp index b06e6e5956..3bc926876a 100644 --- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp @@ -53,6 +53,24 @@ void FlightTaskAutoLine::_generateSetpoints() _generateXYsetpoints(); } +void FlightTaskAutoLine::_setSpeedAtTarget() +{ + // angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 + float angle = 2.0f; // minimum angle + _speed_at_target = 0.0f; + + if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f && + (Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) { + // angle = cos(x) + 1.0 + angle = + Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() + * Vector2f(&(_target - _next_wp)(0)).unit_or_zero() + + 1.0f; + _speed_at_target = math::expontialFromLimits(angle, 0.0f, MPC_CRUISE_90.get(), _mc_cruise_speed); + } +} + + void FlightTaskAutoLine::_generateHeadingAlongTrack() { Vector2f prev_to_dest(_target - _prev_wp); @@ -62,18 +80,22 @@ void FlightTaskAutoLine::_generateHeadingAlongTrack() void FlightTaskAutoLine::_generateXYsetpoints() { + _setSpeedAtTarget(); Vector2f pos_sp_to_dest(_target - _position_setpoint); const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _target_acceptance_radius; 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 */ + // Vehicle reached target in xy and no passing required. Lock position _position_setpoint(0) = _target(0); _position_setpoint(1) = _target(1); _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f; } else { + // Vehicle needs to pass waypoint + // Ensure that vehicle never gets stuck because of too low target-speed + _speed_at_target = math::max(_speed_at_target, 0.5f); // Get various path specific vectors. */ Vector2f u_prev_to_dest = Vector2f(_target - _prev_wp).unit_or_zero(); diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp index 33cbecd006..b6406a6ab4 100644 --- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp @@ -62,4 +62,8 @@ protected: void _generateHeadingAlongTrack(); /**< Generates heading along track. */ void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */ void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */ + +private: + void _setSpeedAtTarget(); /**< Sets desiered speed at target */ + float _speed_at_target = 0.0f; }; diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index bbe177e509..238d8e7306 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -103,7 +103,6 @@ void FlightTaskAutoMapper::_reset() // Set setpoints equal current state. _velocity_setpoint = _velocity; _position_setpoint = _position; - _speed_at_target = 0.0f; } void FlightTaskAutoMapper::_generateIdleSetpoints()