Skip to content

Commit

Permalink
StraightLine: rewrite old implementation
Browse files Browse the repository at this point in the history 
Before it was:
- not used anywhere
- copied from an old mission implementation version
- didn't plan in advance
- had a lot of broken cases
- dependent on a lot of parameters

I'm starting with a new relatively simple implementation that works as
expected for a minimum viable implementation and can be improved over time.
The first version is used to approach the circle path in Orbit mode to
verify the interface and get testing such that it gets eventually used
everywhere.
  • Loading branch information
@MaEtUgR
MaEtUgR committed Aug 8, 2019
1 parent a6777ca commit a7da87d
Show file tree
Hide file tree
Showing 2 changed files with 47 additions and 227 deletions.
209 changes: 29 additions & 180 deletions src/lib/FlightTasks/tasks/Utility/StraightLine.cpp
View file
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -37,205 +37,54 @@

#include "StraightLine.hpp"
#include <mathlib/mathlib.h>
#include <float.h>
#include <px4_defines.h>

#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);

// 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;

float dist_to_target = (_target - _pos).length(); // distance to target

// 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;

} else {
max_acc_hor *= 1000.0f;
}

// 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;

if (fabs(u_orig_to_target(2)) > FLT_EPSILON) {
max_acc_vert /= fabs(u_orig_to_target(2));

} else {
max_acc_vert *= 1000.0f;
}

return math::min(max_acc_hor, max_acc_vert);
}

float StraightLine::getMaxVel()
{
// 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();
Vector3f start_to_end = _end - _start;
float distance_start_to_end = start_to_end.norm();

// calculate the maximal horizontal velocity
float divider = Vector2f(u_orig_to_target).length();
float max_vel_hor = _param_mpc_xy_vel_max.get();
// 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);

if (divider > FLT_EPSILON) {
max_vel_hor /= divider;
float distance_from_boundary = 0.f;

} 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));
// calculate the distance to the closer boundary
if (progress < 0.5f) {
distance_from_boundary = (2 * progress) * distance_start_to_end;

} else {
max_vel_vert *= 1000.0f;
distance_from_boundary = (2 * (1 - progress)) * distance_start_to_end;
}

return math::min(max_vel_hor, max_vel_vert);
}
// 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;

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();
// 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;
}
}

void StraightLine::setSpeed(const float &speed)
void StraightLine::setLineFromTo(const Vector3f &start, const Vector3f &end)
{
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;
}
}
65 changes: 18 additions & 47 deletions src/lib/FlightTasks/tasks/Utility/StraightLine.hpp
View file
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -41,68 +41,39 @@

#pragma once

#include <px4_module_params.h>
#include <matrix/matrix/math.hpp>

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);
void setLineFromTo(const matrix::Vector3f &start, const matrix::Vector3f &end);
void setSpeed(const float &speed) { _speed = speed; };

/**
* 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
* Generate setpoints on a straight line according to parameters
*
* @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
*/
float getMaxVel();
void generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint);

/**
* Generate setpoints on a straight line according to parameters
* Check if the end was reached
*
* @param position_setpoint: 3D vector with the previous position setpoint
* @param velocity_setpoint: 3D vector with the previous velocity setpoint
* @return false when on the way from start to end, true when end was reached
*/
void generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint);
bool isEndReached() { return end_reached; }

private:
const float &_deltatime; /**< delta time between last update (dependency injection) */
const matrix::Vector3f &_pos; /**< 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 */
const matrix::Vector3f &_position; /**< vehicle position (dependency injection) */

// parameters for default values
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max, /**< maximum horizontal acceleration */
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max, /**< maximum vertical acceleration upwards */
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max, /**< maximum vertical acceleration downwards*/
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max, /**< maximum horizontal velocity */
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up, /**< maximum vertical velocity upwards */
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn, /**< maximum vertical velocity downwards */
(ParamFloat<px4::params::NAV_ACC_RAD>) _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 */
};

0 comments on commit a7da87d

Please sign in to comment.