mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 17:13:35 +08:00
StraightLine: rewrite old implementation
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.
This commit is contained in:
Matthias Grob
@@ -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 <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);
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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 <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);
|
||||
|
||||
/**
|
||||
* 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<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 */
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user