diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp index 039a96e96c..f01886f3cb 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.cpp @@ -40,6 +40,9 @@ using namespace matrix; +#define SIGMA_SINGLE_OP 0.000001f +#define SIGMA_NORM 0.001f + bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_array) { if (!FlightTask::initializeSubscriptions(subscription_array)) { @@ -60,7 +63,10 @@ bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_arr bool FlightTaskAuto::activate() { bool ret = FlightTask::activate(); - _prev_prev_wp = _prev_wp = _target = _next_wp = _position; + _position_setpoint = _position; + _velocity_setpoint = _velocity; + _yaw_setpoint = _yaw; + _yawspeed_setpoint = 0.0f; _setDefaultConstraints(); return ret; } @@ -97,47 +103,96 @@ bool FlightTaskAuto::_evaluateTriplets() // Check if triplet is valid. There must be at least a valid altitude. if (!_sub_triplet_setpoint->get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint->get().current.alt)) { - // best we can do is to just set all waypoints to current state and return false - _prev_prev_wp = _prev_wp = _target = _next_wp = _position; + // Best we can do is to just set all waypoints to current state and return false. + _prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position; _type = WaypointType::position; return false; } _type = (WaypointType)_sub_triplet_setpoint->get().current.type; - // always update cruise speed since that can change without waypoint changes + // Always update cruise speed since that can change without waypoint changes. _mc_cruise_speed = _sub_triplet_setpoint->get().current.cruising_speed; if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f) || (_mc_cruise_speed > _constraints.speed_xy)) { - // use default limit + // Use default limit. _mc_cruise_speed = _constraints.speed_xy; } - // get target waypoint. - matrix::Vector3f target; + // Temporary target variable where we save the local reprojection of the latest navigator current triplet. + matrix::Vector3f tmp_target; if (!PX4_ISFINITE(_sub_triplet_setpoint->get().current.lat) || !PX4_ISFINITE(_sub_triplet_setpoint->get().current.lon)) { // No position provided in xy. Lock position if (!PX4_ISFINITE(_lock_position_xy(0))) { - target(0) = _lock_position_xy(0) = _position(0); - target(1) = _lock_position_xy(1) = _position(1); + _triplet_target(0) = _lock_position_xy(0) = _position(0); + _triplet_target(1) = _lock_position_xy(1) = _position(1); } else { - target(0) = _lock_position_xy(0); - target(1) = _lock_position_xy(1); + _triplet_target(0) = _lock_position_xy(0); + _triplet_target(1) = _lock_position_xy(1); _lock_position_xy *= NAN; } } else { // Convert from global to local frame. map_projection_project(&_reference_position, - _sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &target(0), &target(1)); + _sub_triplet_setpoint->get().current.lat, _sub_triplet_setpoint->get().current.lon, &tmp_target(0), &tmp_target(1)); } - target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude); + tmp_target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude); - // check if target is valid + // Check if anything has changed. We do that by comparing the temporary target + // to the internal _triplet_target. + // TODO This is a hack and it would be much better if the navigator only sends out a waypoints once they have changed. + + bool triplet_update = true; + + if (!(fabsf(_triplet_target(0) - tmp_target(0)) > 0.001f || fabsf(_triplet_target(1) - tmp_target(1)) > 0.001f + || fabsf(_triplet_target(2) - tmp_target(2)) > 0.001f)) { + // Nothing has changed: just keep old waypoints. + triplet_update = false; + + } else { + _triplet_target = tmp_target; + + if (!PX4_ISFINITE(_triplet_target(0)) || !PX4_ISFINITE(_triplet_target(1))) { + // Horizontal target is not finite. + _triplet_target(0) = _position(0); + _triplet_target(1) = _position(1); + } + + if (!PX4_ISFINITE(_triplet_target(2))) { + _triplet_target(2) = _position(2); + } + + // If _triplet_target has updated, update also _triplet_prev_wp and _triplet_next_wp. + _prev_prev_wp = _triplet_prev_wp; + + if (_isFinite(_sub_triplet_setpoint->get().previous) && _sub_triplet_setpoint->get().previous.valid) { + map_projection_project(&_reference_position, _sub_triplet_setpoint->get().previous.lat, + _sub_triplet_setpoint->get().previous.lon, &_triplet_prev_wp(0), &_triplet_prev_wp(1)); + _triplet_prev_wp(2) = -(_sub_triplet_setpoint->get().previous.alt - _reference_altitude); + + } else { + _triplet_prev_wp = _position; + } + + if (_type == WaypointType::loiter) { + _triplet_next_wp = _triplet_target; + + } else if (_isFinite(_sub_triplet_setpoint->get().next) && _sub_triplet_setpoint->get().next.valid) { + map_projection_project(&_reference_position, _sub_triplet_setpoint->get().next.lat, + _sub_triplet_setpoint->get().next.lon, &_triplet_next_wp(0), &_triplet_next_wp(1)); + _triplet_next_wp(2) = -(_sub_triplet_setpoint->get().next.alt - _reference_altitude); + + } else { + _triplet_next_wp = _triplet_target; + } + } + + // Check if yaw target is valid. _yaw_setpoint = _sub_triplet_setpoint->get().current.yaw; if (_type == WaypointType::follow_target && _sub_triplet_setpoint->get().current.yawspeed_valid) { @@ -145,51 +200,13 @@ bool FlightTaskAuto::_evaluateTriplets() _yaw_setpoint = NAN; } - // Check if anything has changed. We do that by comparing the target - // setpoint to the previous target. - // TODO This is a hack and it would be much better if the navigator only sends out a waypoints once tthey have changed. + // Calculate the current vehicle state and check if it has updated. + State _previous_state = _current_state; + _current_state = _getCurrentState(); + bool state_update = (_current_state != _previous_state) ? true : false; - // dont't do any updates if the current target has not changed - if (!(fabsf(target(0) - _target(0)) > 0.001f || fabsf(target(1) - _target(1)) > 0.001f - || fabsf(target(2) - _target(2)) > 0.001f)) { - // nothing has changed: just keep old waypoints - return true; - } - - // update all waypoints - _target = target; - - if (!PX4_ISFINITE(_target(0)) || !PX4_ISFINITE(_target(1))) { - // Horizontal target is not finite. */ - _target(0) = _position(0); - _target(1) = _position(1); - } - - if (!PX4_ISFINITE(_target(2))) { - _target(2) = _position(2); - } - - _prev_prev_wp = _prev_wp; - - if (_isFinite(_sub_triplet_setpoint->get().previous) && _sub_triplet_setpoint->get().previous.valid) { - map_projection_project(&_reference_position, _sub_triplet_setpoint->get().previous.lat, - _sub_triplet_setpoint->get().previous.lon, &_prev_wp(0), &_prev_wp(1)); - _prev_wp(2) = -(_sub_triplet_setpoint->get().previous.alt - _reference_altitude); - - } else { - _prev_wp = _position; - } - - if (_type == WaypointType::loiter) { - _next_wp = _target; - - } else if (_isFinite(_sub_triplet_setpoint->get().next) && _sub_triplet_setpoint->get().next.valid) { - map_projection_project(&_reference_position, _sub_triplet_setpoint->get().next.lat, - _sub_triplet_setpoint->get().next.lon, &_next_wp(0), &_next_wp(1)); - _next_wp(2) = -(_sub_triplet_setpoint->get().next.alt - _reference_altitude); - - } else { - _next_wp = _target; + if (triplet_update || state_update) { + _updateInternalWaypoints(); } return true; @@ -252,3 +269,194 @@ matrix::Vector2f FlightTaskAuto::_getTargetVelocityXY() return matrix::Vector2f{}; } } + +State FlightTaskAuto::_getCurrentState() +{ + // Calculate the vehicle current state based on the Navigator triplets and the current position. + Vector2f u_prev_to_target = Vector2f(&(_triplet_target - _triplet_prev_wp)(0)).unit_or_zero(); + Vector2f pos_to_target = Vector2f(&(_triplet_target - _position)(0)); + Vector2f prev_to_pos = Vector2f(&(_position - _triplet_prev_wp)(0)); + _closest_pt = Vector2f(&_triplet_prev_wp(0)) + u_prev_to_target * (prev_to_pos * u_prev_to_target); + + State return_state = State::none; + + if (u_prev_to_target * pos_to_target < 0.0f) { + // Target is behind. + return_state = State::target_behind; + + } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) { + // Current position is more than cruise speed in front of previous setpoint. + return_state = State::previous_infront; + + } else if (Vector2f(Vector2f(&_position(0)) - _closest_pt).length() > _mc_cruise_speed) { + // Vehicle is more than cruise speed off track. + return_state = State::offtrack; + + } + + return return_state; +} + + +void FlightTaskAuto::_updateInternalWaypoints() +{ + // The internal Waypoints might differ from _triplet_prev_wp, _triplet_target and _triplet_next_wp. + // The cases where it differs: + // 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)(0)).length() > 0.001f && + (Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) { + + angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() + * Vector2f(&(_target - _next_wp)(0)).unit_or_zero() + + 1.0f; + _speed_at_target = _getVelocityFromAngle(angle); + } + } + break; + + case State::previous_infront: { + _next_wp = _triplet_target; + _target = _triplet_prev_wp; + _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)(0)).length() > 0.001f && + (Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) { + + angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() + * Vector2f(&(_target - _next_wp)(0)).unit_or_zero() + + 1.0f; + _speed_at_target = _getVelocityFromAngle(angle); + } + } + break; + + case State::offtrack: { + _next_wp = _triplet_target; + _target = matrix::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)(0)).length() > 0.001f && + (Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) { + + angle = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() + * Vector2f(&(_target - _next_wp)(0)).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; + + 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)(0)).length() > 0.001f && + (Vector2f(&(_target - _prev_wp)(0)).length() > NAV_ACC_RAD.get())) { + + angle = + Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() + * Vector2f(&(_target - _next_wp)(0)).unit_or_zero() + + 1.0f; + _speed_at_target = _getVelocityFromAngle(angle); + } + + break; + } + + default: + break; + } +} + +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); +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp index 8e8d259654..065737706d 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAuto.hpp @@ -61,6 +61,13 @@ enum class WaypointType : int { follow_target }; +enum class State { + offtrack, /**< Vehicle is more than cruise speed away from track */ + target_behind, /**< Vehicle is in front of target. */ + previous_infront, /**< Vehilce is behind previous waypoint.*/ + none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */ +}; + class FlightTaskAuto : public FlightTask { public: @@ -75,6 +82,7 @@ protected: void _setDefaultConstraints() override; float _getMaxCruiseSpeed() {return MPC_XY_CRUISE.get();} /**< getter for default cruise speed */ matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/ + void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */ matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */ @@ -84,13 +92,28 @@ protected: WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */ uORB::Subscription *_sub_home_position{nullptr}; + + State _current_state{State::none}; + + float _speed_at_target = 0.0f; /**< Desired velocity at 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 + ); /**< Default mc cruise speed.*/ + + private: matrix::Vector2f _lock_position_xy{NAN, NAN}; uORB::Subscription *_sub_triplet_setpoint{nullptr}; - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, - (ParamFloat) MPC_XY_CRUISE); /**< Default mc cruise speed.*/ + matrix::Vector3f _triplet_target; + matrix::Vector3f _triplet_prev_wp; + matrix::Vector3f _triplet_next_wp; + matrix::Vector2f _closest_pt; + map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */ float _reference_altitude = NAN; /**< Altitude relative to ground. */ @@ -99,4 +122,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(); }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index f35b955931..fde1ec11fd 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -51,7 +51,6 @@ bool FlightTaskAutoLine::activate() void FlightTaskAutoLine::_generateSetpoints() { - _updateInternalWaypoints(); _generateAltitudeSetpoints(); _generateXYsetpoints(); @@ -62,141 +61,29 @@ void FlightTaskAutoLine::_generateSetpoints() } } -void FlightTaskAutoLine::_updateInternalWaypoints() -{ - // The internal Waypoints might differ from previous_wp and target. The cases where it differs: - // 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. - // Adjust destination and origin based on current vehicle state. - Vector2f u_prev_to_target = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero(); - Vector2f pos_to_target = Vector2f(&(_target - _position)(0)); - Vector2f prev_to_pos = Vector2f(&(_position - _prev_wp)(0)); - Vector2f closest_pt = Vector2f(&_prev_wp(0)) + u_prev_to_target * (prev_to_pos * u_prev_to_target); - - if (u_prev_to_target * pos_to_target < 0.0f) { - - // Target is behind. */ - if (_current_state != State::target_behind) { - - _destination = _target; - _origin = _position; - _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(&(_destination - _next_wp)(0)).length() > 0.001f && - (Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) { - - angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero() - * Vector2f(&(_destination - _next_wp)(0)).unit_or_zero() - + 1.0f; - _speed_at_target = _getVelocityFromAngle(angle); - } - } - - } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.length() > _mc_cruise_speed) { - - // Current position is more than cruise speed in front of previous setpoint. - if (_current_state != State::previous_infront) { - _destination = _prev_wp; - _origin = _position; - _current_state = State::previous_infront; - - 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(&(_destination - _next_wp)(0)).length() > 0.001f && - (Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) { - - angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero() - * Vector2f(&(_destination - _next_wp)(0)).unit_or_zero() - + 1.0f; - _speed_at_target = _getVelocityFromAngle(angle); - } - - } - - } else if (Vector2f(Vector2f(&_position(0)) - closest_pt).length() > _mc_cruise_speed) { - - // Vehicle is more than cruise speed off track. - if (_current_state != State::offtrack) { - _destination = matrix::Vector3f(closest_pt(0), closest_pt(1), _target(2)); - _origin = _position; - _current_state = State::offtrack; - - 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(&(_destination - _next_wp)(0)).length() > 0.001f && - (Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) { - - angle = Vector2f(&(_destination - _origin)(0)).unit_or_zero() - * Vector2f(&(_destination - _next_wp)(0)).unit_or_zero() - + 1.0f; - _speed_at_target = _getVelocityFromAngle(angle); - } - - } - - } else { - - if ((_target - _destination).length() > 0.01f) { - // A new target is available. Update speed at target.*/ - _destination = _target; - _origin = _prev_wp; - _current_state = State::none; - - 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(&(_destination - _next_wp)(0)).length() > 0.001f && - (Vector2f(&(_destination - _origin)(0)).length() > NAV_ACC_RAD.get())) { - - angle = - Vector2f(&(_destination - _origin)(0)).unit_or_zero() - * Vector2f(&(_destination - _next_wp)(0)).unit_or_zero() - + 1.0f; - _speed_at_target = _getVelocityFromAngle(angle); - } - } - } -} void FlightTaskAutoLine::_generateXYsetpoints() { - Vector2f pos_sp_to_dest = Vector2f(&(_destination - _position_setpoint)(0)); - const bool has_reached_altitude = fabsf(_destination(2) - _position(2)) < NAV_ACC_RAD.get(); + Vector2f pos_sp_to_dest = Vector2f(&(_target - _position_setpoint)(0)); + const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < NAV_ACC_RAD.get(); 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())) { // Vehicle reached target in xy and no passing required. Lock position */ - _position_setpoint(0) = _destination(0); - _position_setpoint(1) = _destination(1); + _position_setpoint(0) = _target(0); + _position_setpoint(1) = _target(1); _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f; } else { // Get various path specific vectors. */ - Vector2f u_prev_to_dest = Vector2f(&(_destination - _origin)(0)).unit_or_zero(); - Vector2f prev_to_pos(&(_position - _origin)(0)); - Vector2f closest_pt = Vector2f(&_origin(0)) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest); - Vector2f closest_to_dest = Vector2f(&(_destination - _position)(0)); - Vector2f prev_to_dest = Vector2f(&(_destination - _origin)(0)); + Vector2f u_prev_to_dest = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero(); + Vector2f prev_to_pos(&(_position - _prev_wp)(0)); + Vector2f closest_pt = Vector2f(&_prev_wp(0)) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest); + Vector2f closest_to_dest = Vector2f(&(_target - _position)(0)); + Vector2f prev_to_dest = Vector2f(&(_target - _prev_wp)(0)); float speed_sp_track = _mc_cruise_speed; float speed_sp_prev_track = math::max(Vector2f(&_velocity_setpoint(0)) * u_prev_to_dest, 0.0f); @@ -286,17 +173,17 @@ void FlightTaskAutoLine::_generateXYsetpoints() void FlightTaskAutoLine::_generateAltitudeSetpoints() { // Total distance between previous and target set-point. - const float dist = fabsf(_destination(2) - _origin(2)); + const float dist = fabsf(_target(2) - _prev_wp(2)); // If target has not been reached, then compute set-point depending on maximum velocity. - if ((dist > SIGMA_NORM) && (fabsf(_position(2) - _destination(2)) > 0.1f)) { + if ((dist > SIGMA_NORM) && (fabsf(_position(2) - _target(2)) > 0.1f)) { // get various distances */ - const float dist_to_prev = fabsf(_position(2) - _origin(2)); - const float dist_to_target = fabsf(_destination(2) - _position(2)); + const float dist_to_prev = fabsf(_position(2) - _prev_wp(2)); + const float dist_to_target = fabsf(_target(2) - _position(2)); // check sign - const bool flying_upward = _destination(2) < _position(2); + const bool flying_upward = _target(2) < _position(2); // limit vertical downwards speed (positive z) close to ground // for now we use the altitude above home and assume that we want to land at same height as we took off @@ -356,67 +243,10 @@ void FlightTaskAutoLine::_generateAltitudeSetpoints() // vehicle reached desired target altitude _velocity_setpoint(2) = 0.0f; - _position_setpoint(2) = _destination(2); + _position_setpoint(2) = _target(2); } } -float FlightTaskAutoLine::_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); -} bool FlightTaskAutoLine::_highEnoughForLandingGear() { diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp index fdfd76409d..96973e5b5f 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp @@ -53,18 +53,18 @@ protected: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper, (ParamFloat) MIS_YAW_ERR, // yaw-error threshold + (ParamFloat) MPC_ACC_HOR, // acceleration in flight (ParamFloat) MPC_ACC_UP_MAX, - (ParamFloat) MPC_ACC_DOWN_MAX, - (ParamFloat) MPC_ACC_HOR // acceleration in flight - ) + (ParamFloat) MPC_ACC_DOWN_MAX + ); void _generateSetpoints() override; - void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ + 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: - float _getVelocityFromAngle(const float angle); /**< Computes the speed at target depending on angle. */ + bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */ }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp index 9906ce61d8..e943039784 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp @@ -40,8 +40,7 @@ using namespace matrix; -#define SIGMA_SINGLE_OP 0.000001f -#define SIGMA_NORM 0.001f + bool FlightTaskAutoMapper::activate() { @@ -100,9 +99,6 @@ void FlightTaskAutoMapper::_reset() // Set setpoints equal current state. _velocity_setpoint = _velocity; _position_setpoint = _position; - _yaw_setpoint = _yaw; - _destination = _position; - _origin = _position; _speed_at_target = 0.0f; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp index 24c4063f56..8fddf5b7ea 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp @@ -53,28 +53,15 @@ public: protected: - matrix::Vector3f _destination{}; /**< Current target. Is not necessarily the same as triplet target. */ - matrix::Vector3f _origin{}; /**< Previous waypoint. Is not necessarily the same as triplet previous. */ - float _speed_at_target = 0.0f; /**< Desired velocity at target. */ float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */ - enum class State { - offtrack, /**< Vehicle is more than cruise speed away from track */ - target_behind, /**< Vehicle is in front of target. */ - previous_infront, /**< Vehilce is behind previous waypoint.*/ - none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */ - }; - State _current_state{State::none}; - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, (ParamFloat) MPC_LAND_SPEED, - (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) MPC_TILTMAX_LND, (ParamFloat) MPC_LAND_ALT1, // altitude at which speed limit downwards reaches maximum speed (ParamFloat) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed - (ParamFloat) MPC_TILTMAX_LND, (ParamFloat) MPC_TKO_SPEED - ) + ); virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */