fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 15:19:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

trajectory-planning: Added option for PositionSmoothing library with single target waypoint that disables L1 guidance

Browse Source
This commit is contained in:
Thomas Debrunner 2021-10-20 15:59:52 +02:00 committed by Daniel Agar
parent 77af102cab
commit ade4a1930c
3 changed files with 55 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/lib/motion_planning/PositionSmoothing.cpp
View File

@ -38,9 +38,10 @@
#include <matrix/matrix/helper_functions.hpp>
void PositionSmoothing::generateSetpoints(
void PositionSmoothing::_generateSetpoints(
const Vector3f &position,
const Vector3f(&waypoints)[3],
bool is_single_waypoint,
const Vector3f &feedforward_velocity,
float delta_time,
bool force_zero_velocity_setpoint,
@ -49,7 +50,7 @@ void PositionSmoothing::generateSetpoints(
Vector3f velocity_setpoint{0.f, 0.f, 0.f};
if (!force_zero_velocity_setpoint) {
velocity_setpoint = _generateVelocitySetpoint(position, waypoints, feedforward_velocity);
velocity_setpoint = _generateVelocitySetpoint(position, waypoints, is_single_waypoint, feedforward_velocity);
}
out_setpoints.unsmoothed_velocity = velocity_setpoint;
@ -184,6 +185,7 @@ const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve
}
const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],
bool is_single_waypoint,
const Vector3f &feedforward_velocity_setpoint)
{
// Interface: A valid position setpoint generates a velocity target using conservative motion constraints.
@ -201,13 +203,13 @@ const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &posi
Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
_trajectory[1].getCurrentPosition(),
_trajectory[2].getCurrentPosition());
const Vector3f u_pos_traj_to_dest((_getCrossingPoint(position, waypoints) - pos_traj).unit_or_zero());
const Vector3f crossing_point = is_single_waypoint ? target : _getCrossingPoint(position, waypoints);
const Vector3f u_pos_traj_to_dest{(crossing_point - pos_traj).unit_or_zero()};
float xy_speed = _getMaxXYSpeed(waypoints);
const float z_speed = _getMaxZSpeed(waypoints);
if (_isTurning(target)) {
if (!is_single_waypoint && _isTurning(target)) {
// Limit speed during a turn
xy_speed = math::min(_max_speed_previous, xy_speed);
@ -235,7 +237,8 @@ const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &posi
// Get various path specific vectors
Vector2f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
Vector2f pos_traj_to_dest_xy = Vector2f(_getCrossingPoint(position, waypoints)) - pos_traj;
Vector2f crossing_point = is_single_waypoint ? Vector2f(target) : Vector2f(_getCrossingPoint(position, waypoints));
Vector2f pos_traj_to_dest_xy = crossing_point - pos_traj;
Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
float xy_speed = _getMaxXYSpeed(waypoints);

48
src/lib/motion_planning/PositionSmoothing.hpp
View File

@ -68,7 +68,7 @@ public:
/**
* @brief Generates new setpoints for jerk, acceleration, velocity and position
* to reach the given waypoint smoothly from current position.
* to reach the given waypoint triplet smoothly from current position
*
* @param position Current position of the vehicle
* @param waypoints 0: Past waypoint, 1: target, 2: Target after next target
@ -77,14 +77,44 @@ public:
* @param force_zero_velocity_setpoint Force vehicle to stop. Generate trajectory that ends with still vehicle.
* @param out_setpoints Output of the generated setpoints
*/
void generateSetpoints(
inline void generateSetpoints(
const Vector3f &position,
const Vector3f(&waypoints)[3],
const Vector3f &feedforward_velocity,
float delta_time,
bool force_zero_velocity_setpoint,
PositionSmoothingSetpoints &out_setpoints
);
)
{
_generateSetpoints(position, waypoints, false, feedforward_velocity, delta_time, force_zero_velocity_setpoint,
out_setpoints);
}
/**
* @brief Generates new setpoints for jerk, acceleration, velocity and position
* to reach the given waypoint smoothly from current position.
*
* @param position Current position of the vehicle
* @param waypoint desired waypoint
* @param feedforward_velocity FF velocity
* @param delta_time Time since last invocation of the function
* @param force_zero_velocity_setpoint Force vehicle to stop. Generate trajectory that ends with still vehicle.
* @param out_setpoints Output of the generated setpoints
*/
inline void generateSetpoints(
const Vector3f &position,
const Vector3f &waypoint,
const Vector3f &feedforward_velocity,
float delta_time,
bool force_zero_velocity_setpoint,
PositionSmoothingSetpoints &out_setpoints
)
{
Vector3f waypoints[3] = {waypoint, waypoint, waypoint};
_generateSetpoints(position, waypoints, true, feedforward_velocity, delta_time, force_zero_velocity_setpoint,
out_setpoints);
}
/**
* @brief Reset internal state to the given values
@ -394,7 +424,19 @@ private:
/* Internal functions */
bool _isTurning(const Vector3f &target) const;
void _generateSetpoints(
const Vector3f &position,
const Vector3f(&waypoints)[3],
bool is_single_waypoint,
const Vector3f &feedforward_velocity,
float delta_time,
bool force_zero_velocity_setpoint,
PositionSmoothingSetpoints &out_setpoints
);
const Vector3f _generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],
bool is_single_waypoint,
const Vector3f &feedforward_velocity_setpoint);
const Vector2f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
const Vector3f _getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const;

4
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -282,9 +282,7 @@ void FlightTaskOrbit::_generate_circle_approach_setpoints()
const Vector3f target_circle_point{closest_point_on_circle(0), closest_point_on_circle(1), _center(2)};
PositionSmoothing::PositionSmoothingSetpoints out_setpoints;
_position_smoothing.generateSetpoints(_position, {
_circle_approach_start_position, target_circle_point, target_circle_point
},
_position_smoothing.generateSetpoints(_position, target_circle_point,
{0.f, 0.f, 0.f}, _deltatime, false, out_setpoints);
_yaw_setpoint = atan2f(position_to_center_xy(1), position_to_center_xy(0));