diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp index 98936eb577..078564c018 100644 --- a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp +++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,205 +37,54 @@ #include "StraightLine.hpp" #include -#include #include -#define VEL_ZERO_THRESHOLD 0.001f // Threshold to compare if the target velocity is zero -#define DECELERATION_MAX 8.0f // The vehicles maximum deceleration TODO check to create param - using namespace matrix; -StraightLine::StraightLine(ModuleParams *parent, const float &deltatime, const matrix::Vector3f &pos) : - ModuleParams(parent), - _deltatime(deltatime), _pos(pos) -{ - -} - void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint) { - // Check if target position has been reached - if (_desired_speed_at_target < VEL_ZERO_THRESHOLD && - (_pos - _target).length() < _param_nav_acc_rad.get()) { - // Vehicle has reached target. Lock position - position_setpoint = _target; - velocity_setpoint = Vector3f(0.0f, 0.0f, 0.0f); - + if (isEndReached()) { + // Vehicle has reached target, lock position + position_setpoint = _end; + velocity_setpoint.setNaN(); return; } - // unit vector in the direction of the straight line - Vector3f u_orig_to_target = (_target - _origin).unit_or_zero(); - // vector from origin to current position - Vector3f orig_to_pos = _pos - _origin; - // current position projected perpendicularly onto desired line - Vector3f closest_pt_on_line = _origin + u_orig_to_target * (orig_to_pos * u_orig_to_target); - // previous velocity in the direction of the line - float speed_sp_prev = math::max(velocity_setpoint * u_orig_to_target, 0.0f); + Vector3f start_to_end = _end - _start; + float distance_start_to_end = start_to_end.norm(); - // Calculate accelerating/decelerating distance depending on speed, speed at target and acceleration/deceleration - float acc_dec_distance = fabs((_desired_speed * _desired_speed) - (_desired_speed_at_target * - _desired_speed_at_target)) / 2.0f; - acc_dec_distance /= _desired_speed > _desired_speed_at_target ? _desired_deceleration : _desired_acceleration; + // capture progress as ratio between 0 and 1 of entire distance + Vector3f vehicle_to_end = _end - _position; + float distance_vehicle_to_end = vehicle_to_end.norm(); + float distance_from_start = Vector3f(_start - _position).norm(); + float progress = distance_from_start / (distance_from_start + distance_vehicle_to_end); - float dist_to_target = (_target - _pos).length(); // distance to target + float distance_from_boundary = 0.f; - // Either accelerate or decelerate - float speed_sp = dist_to_target > acc_dec_distance ? _desired_speed : _desired_speed_at_target; - float max_acc_dec = speed_sp > speed_sp_prev ? _desired_acceleration : -_desired_deceleration; - - float acc_track = 0.0f; - - if (_deltatime > FLT_EPSILON) { - acc_track = (speed_sp - speed_sp_prev) / _deltatime; - } - - if (fabs(acc_track) > fabs(max_acc_dec)) { - // accelerate/decelerate with desired acceleration/deceleration towards target - speed_sp = speed_sp_prev + max_acc_dec * _deltatime; - } - - // constrain the velocity - speed_sp = math::constrain(speed_sp, 0.0f, _desired_speed); - - // set the position and velocity setpoints - position_setpoint = closest_pt_on_line; - velocity_setpoint = u_orig_to_target * speed_sp; -} - -float StraightLine::getMaxAcc() -{ - // check if origin and target are different points - if ((_target - _origin).length() < FLT_EPSILON) { - return _param_mpc_acc_hor_max.get(); - } - - // unit vector in the direction of the straight line - Vector3f u_orig_to_target = (_target - _origin).unit_or_zero(); - - // calculate the maximal horizontal acceleration - float divider = Vector2f(u_orig_to_target).length(); - float max_acc_hor = _param_mpc_acc_hor_max.get(); - - if (divider > FLT_EPSILON) { - max_acc_hor /= divider; + // calculate the distance to the closer boundary + if (progress < 0.5f) { + distance_from_boundary = (2 * progress) * distance_start_to_end; } else { - max_acc_hor *= 1000.0f; + distance_from_boundary = (2 * (1 - progress)) * distance_start_to_end; } - // calculate the maximal vertical acceleration - float max_acc_vert_original = u_orig_to_target(2) < 0 ? _param_mpc_acc_up_max.get() : _param_mpc_acc_down_max.get(); - float max_acc_vert = max_acc_vert_original; + // ramp velocity based on the distance to the boundary + float velocity = 0.5f + (distance_from_boundary / 4.f); + velocity = math::min(velocity, _speed); + velocity_setpoint = vehicle_to_end.unit_or_zero() * velocity; - if (fabs(u_orig_to_target(2)) > FLT_EPSILON) { - max_acc_vert /= fabs(u_orig_to_target(2)); - - } else { - max_acc_vert *= 1000.0f; + // check if we plan to go against the line direction which indicates we reached the goal + if (start_to_end * vehicle_to_end < 0) { + end_reached = true; } - - return math::min(max_acc_hor, max_acc_vert); } -float StraightLine::getMaxVel() +void StraightLine::setLineFromTo(const Vector3f &start, const Vector3f &end) { - // check if origin and target are different points - if ((_target - _origin).length() < FLT_EPSILON) { - return _param_mpc_xy_vel_max.get(); - } - - // unit vector in the direction of the straight line - Vector3f u_orig_to_target = (_target - _origin).unit_or_zero(); - - // calculate the maximal horizontal velocity - float divider = Vector2f(u_orig_to_target).length(); - float max_vel_hor = _param_mpc_xy_vel_max.get(); - - if (divider > FLT_EPSILON) { - max_vel_hor /= divider; - - } else { - max_vel_hor *= 1000.0f; - } - - // calculate the maximal vertical velocity - float max_vel_vert_directional = u_orig_to_target(2) < 0 ? _param_mpc_z_vel_max_up.get() : - _param_mpc_z_vel_max_dn.get(); - float max_vel_vert = max_vel_vert_directional; - - if (fabs(u_orig_to_target(2)) > FLT_EPSILON) { - max_vel_vert /= fabs(u_orig_to_target(2)); - - } else { - max_vel_vert *= 1000.0f; - } - - return math::min(max_vel_hor, max_vel_vert); -} - -void StraightLine::setAllDefaults() -{ - _desired_speed = getMaxVel(); - _desired_speed_at_target = 0.0f; - _desired_acceleration = getMaxAcc(); - _desired_deceleration = DECELERATION_MAX; -} - -void StraightLine::setLineFromTo(const matrix::Vector3f &origin, const matrix::Vector3f &target) -{ - if (PX4_ISFINITE(target(0)) && PX4_ISFINITE(target(1)) && PX4_ISFINITE(target(2)) && - PX4_ISFINITE(origin(0)) && PX4_ISFINITE(origin(1)) && PX4_ISFINITE(origin(2))) { - _target = target; - _origin = origin; - - // set all parameters to their default value (depends on the direction) - setAllDefaults(); - } -} - -void StraightLine::setSpeed(const float &speed) -{ - float vel_max = getMaxVel(); - - if (speed > FLT_EPSILON && speed < vel_max) { - _desired_speed = speed; - - } else if (speed > vel_max) { - _desired_speed = vel_max; - } -} - -void StraightLine::setSpeedAtTarget(const float &speed_at_target) -{ - float vel_max = getMaxVel(); - - if (speed_at_target > FLT_EPSILON && speed_at_target < vel_max) { - _desired_speed_at_target = speed_at_target; - - } else if (speed_at_target > vel_max) { - _desired_speed_at_target = vel_max; - } -} - -void StraightLine::setAcceleration(const float &acc) -{ - float acc_max = getMaxVel(); - - if (acc > FLT_EPSILON && acc < acc_max) { - _desired_acceleration = acc; - - } else if (acc > acc_max) { - _desired_acceleration = acc_max; - } -} - -void StraightLine::setDeceleration(const float &dec) -{ - if (dec > FLT_EPSILON && dec < DECELERATION_MAX) { - _desired_deceleration = dec; - - } else if (dec > DECELERATION_MAX) { - _desired_deceleration = DECELERATION_MAX; + if (PX4_ISFINITE(start.norm_squared()) && PX4_ISFINITE(end.norm_squared())) { + _start = start; + _end = end; + end_reached = false; } } diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp index eaefdb4b42..5097999cde 100644 --- a/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp +++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,68 +41,39 @@ #pragma once -#include #include -class StraightLine : public ModuleParams +class StraightLine { public: - StraightLine(ModuleParams *parent, const float &deltatime, const matrix::Vector3f &pos); + StraightLine(const matrix::Vector3f &pos) : _position(pos) {}; ~StraightLine() = default; // setter functions - void setLineFromTo(const matrix::Vector3f &origin, const matrix::Vector3f &target); - void setSpeed(const float &speed); - void setSpeedAtTarget(const float &speed_at_target); - void setAcceleration(const float &acc); - void setDeceleration(const float &dec); - - /** - * Set all parameters to their default value depending on the direction of the line - */ - void setAllDefaults(); - - /** - * Get the maximum possible acceleration depending on the direction of the line - * Respects horizontal and vertical limits - */ - float getMaxAcc(); - - /** - * Get the maximum possible velocity depending on the direction of the line - * Respects horizontal and vertical limits - */ - float getMaxVel(); + void setLineFromTo(const matrix::Vector3f &start, const matrix::Vector3f &end); + void setSpeed(const float &speed) { _speed = speed; }; /** * Generate setpoints on a straight line according to parameters * - * @param position_setpoint: 3D vector with the previous position setpoint - * @param velocity_setpoint: 3D vector with the previous velocity setpoint + * @param position_setpoint: reference to the 3D vector with the position setpoint to update + * @param velocity_setpoint: reference to the 3D vector with the velocity setpoint to update */ void generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint); + /** + * Check if the end was reached + * + * @return false when on the way from start to end, true when end was reached + */ + bool isEndReached() { return end_reached; } + private: - const float &_deltatime; /**< delta time between last update (dependency injection) */ - const matrix::Vector3f &_pos; /**< vehicle position (dependency injection) */ + const matrix::Vector3f &_position; /**< vehicle position (dependency injection) */ - float _desired_acceleration{0.0f}; /**< acceleration along the straight line */ - float _desired_deceleration{0.0f}; /**< deceleration along the straight line */ - float _desired_speed{0.0f}; /**< desired maximum velocity */ - float _desired_speed_at_target{0.0f}; /**< desired velocity at target point */ - - matrix::Vector3f _target{}; /**< End point of the straight line */ - matrix::Vector3f _origin{}; /**< Start point of the straight line */ - - // parameters for default values - DEFINE_PARAMETERS( - (ParamFloat) _param_mpc_acc_hor_max, /**< maximum horizontal acceleration */ - (ParamFloat) _param_mpc_acc_up_max, /**< maximum vertical acceleration upwards */ - (ParamFloat) _param_mpc_acc_down_max, /**< maximum vertical acceleration downwards*/ - (ParamFloat) _param_mpc_xy_vel_max, /**< maximum horizontal velocity */ - (ParamFloat) _param_mpc_z_vel_max_up, /**< maximum vertical velocity upwards */ - (ParamFloat) _param_mpc_z_vel_max_dn, /**< maximum vertical velocity downwards */ - (ParamFloat) _param_nav_acc_rad /**< acceptance radius if a waypoint is reached */ - ) + matrix::Vector3f _start; /**< Start point of the straight line */ + matrix::Vector3f _end; /**< End point of the straight line */ + float _speed = 1.f; /**< desired speed between accelerating and decelerating */ + bool end_reached = true; /**< Flag to lock further movement when end is reached */ };