mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-05 18:30:35 +08:00
lib: name folders consistently
This commit is contained in:
Daniel Agar
@@ -0,0 +1,370 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file FlightAutoLine.cpp
|
||||
*/
|
||||
|
||||
#include "FlightTaskAutoLineSmoothVel.hpp"
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <float.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s last_setpoint)
|
||||
{
|
||||
bool ret = FlightTaskAutoMapper2::activate(last_setpoint);
|
||||
|
||||
checkSetpoints(last_setpoint);
|
||||
const Vector3f vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz);
|
||||
const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z);
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_trajectory[i].reset(last_setpoint.acceleration[i], vel_prev(i), pos_prev(i));
|
||||
}
|
||||
|
||||
_yaw_sp_prev = last_setpoint.yaw;
|
||||
_updateTrajConstraints();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::reActivate()
|
||||
{
|
||||
// On ground, reset acceleration and velocity to zero
|
||||
for (int i = 0; i < 2; ++i) {
|
||||
_trajectory[i].reset(0.f, 0.f, _position(i));
|
||||
}
|
||||
|
||||
_trajectory[2].reset(0.f, 0.7f, _position(2));
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
|
||||
{
|
||||
// If the position setpoint is unknown, set to the current postion
|
||||
if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); }
|
||||
|
||||
if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); }
|
||||
|
||||
if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
|
||||
|
||||
// If the velocity setpoint is unknown, set to the current velocity
|
||||
if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); }
|
||||
|
||||
if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); }
|
||||
|
||||
if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
|
||||
|
||||
// No acceleration estimate available, set to zero if the setpoint is NAN
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; }
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
|
||||
}
|
||||
|
||||
/**
|
||||
* EKF reset handling functions
|
||||
* Those functions are called by the base FlightTask in
|
||||
* case of an EKF reset event
|
||||
*/
|
||||
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionXY()
|
||||
{
|
||||
_trajectory[0].setCurrentPosition(_position(0));
|
||||
_trajectory[1].setCurrentPosition(_position(1));
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityXY()
|
||||
{
|
||||
_trajectory[0].setCurrentVelocity(_velocity(0));
|
||||
_trajectory[1].setCurrentVelocity(_velocity(1));
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerPositionZ()
|
||||
{
|
||||
_trajectory[2].setCurrentPosition(_position(2));
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerVelocityZ()
|
||||
{
|
||||
_trajectory[2].setCurrentVelocity(_velocity(2));
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi)
|
||||
{
|
||||
_yaw_sp_prev += delta_psi;
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_generateSetpoints()
|
||||
{
|
||||
_prepareSetpoints();
|
||||
_generateTrajectory();
|
||||
|
||||
if (!PX4_ISFINITE(_yaw_setpoint) && !PX4_ISFINITE(_yawspeed_setpoint)) {
|
||||
// no valid heading -> generate heading in this flight task
|
||||
_generateHeading();
|
||||
}
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_generateHeading()
|
||||
{
|
||||
// Generate heading along trajectory if possible, otherwise hold the previous yaw setpoint
|
||||
if (!_generateHeadingAlongTraj()) {
|
||||
_yaw_setpoint = _yaw_sp_prev;
|
||||
}
|
||||
}
|
||||
|
||||
bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj()
|
||||
{
|
||||
bool res = false;
|
||||
Vector2f vel_sp_xy(_velocity_setpoint);
|
||||
Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position);
|
||||
|
||||
if ((vel_sp_xy.length() > .1f) &&
|
||||
(traj_to_target.length() > _target_acceptance_radius)) {
|
||||
// Generate heading from velocity vector, only if it is long enough
|
||||
// and if the drone is far enough from the target
|
||||
_compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy);
|
||||
res = true;
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
/* Constrain some value vith a constrain depending on the sign of the constraint
|
||||
* Example: - if the constrain is -5, the value will be constrained between -5 and 0
|
||||
* - if the constrain is 5, the value will be constrained between 0 and 5
|
||||
*/
|
||||
float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint)
|
||||
{
|
||||
const float min = (constraint < FLT_EPSILON) ? constraint : 0.f;
|
||||
const float max = (constraint > FLT_EPSILON) ? constraint : 0.f;
|
||||
|
||||
return math::constrain(val, min, max);
|
||||
}
|
||||
|
||||
float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float max)
|
||||
{
|
||||
return math::sign(val) * math::min(fabsf(val), fabsf(max));
|
||||
}
|
||||
|
||||
float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget(float next_target_speed) const
|
||||
{
|
||||
// Compute the maximum allowed speed at the waypoint assuming that we want to
|
||||
// connect the two lines (prev-current and current-next)
|
||||
// with a tangent circle with constant speed and desired centripetal acceleration: a_centripetal = speed^2 / radius
|
||||
// The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius.
|
||||
// This is not exactly true in reality since Navigator switches the waypoint so we have to take in account that
|
||||
// the real acceptance radius is smaller.
|
||||
// It can be that the next waypoint is the last one or that the drone will have to stop for some other reason
|
||||
// so we have to make sure that the speed at the current waypoint allows to stop at the next waypoint.
|
||||
float speed_at_target = 0.0f;
|
||||
|
||||
const float distance_current_next = (_target - _next_wp).xy().norm();
|
||||
const bool waypoint_overlap = (_target - _prev_wp).xy().norm() < _target_acceptance_radius;
|
||||
const bool yaw_align_check_pass = (_param_mpc_yaw_mode.get() != 4) || _yaw_sp_aligned;
|
||||
|
||||
|
||||
if (distance_current_next > 0.001f &&
|
||||
!waypoint_overlap &&
|
||||
yaw_align_check_pass) {
|
||||
Vector3f pos_traj;
|
||||
pos_traj(0) = _trajectory[0].getCurrentPosition();
|
||||
pos_traj(1) = _trajectory[1].getCurrentPosition();
|
||||
pos_traj(2) = _trajectory[2].getCurrentPosition();
|
||||
// Max speed between current and next
|
||||
const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next, next_target_speed);
|
||||
const float alpha = acosf(Vector2f((_target - pos_traj).xy()).unit_or_zero().dot(
|
||||
Vector2f((_target - _next_wp).xy()).unit_or_zero()));
|
||||
// We choose a maximum centripetal acceleration of MPC_ACC_HOR * MPC_XY_TRAJ_P to take in account
|
||||
// that there is a jerk limit (a direct transition from line to circle is not possible)
|
||||
// MPC_XY_TRAJ_P should be between 0 and 1.
|
||||
float accel_tmp = _param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get();
|
||||
float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(alpha,
|
||||
accel_tmp,
|
||||
_target_acceptance_radius);
|
||||
speed_at_target = math::min(math::min(max_speed_in_turn, max_speed_current_next), _mc_cruise_speed);
|
||||
}
|
||||
|
||||
return speed_at_target;
|
||||
}
|
||||
|
||||
float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance, float final_speed) const
|
||||
{
|
||||
float max_speed = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_auto.get(),
|
||||
_param_mpc_acc_hor.get(),
|
||||
braking_distance,
|
||||
final_speed);
|
||||
|
||||
return max_speed;
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
|
||||
{
|
||||
// Interface: A valid position setpoint generates a velocity target using a P controller. If a velocity is specified
|
||||
// that one is used as a velocity limit.
|
||||
// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
|
||||
|
||||
_want_takeoff = false;
|
||||
|
||||
if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
|
||||
// Wait for the yaw setpoint to be aligned
|
||||
_velocity_setpoint.setAll(0.f);
|
||||
|
||||
} else {
|
||||
if (PX4_ISFINITE(_position_setpoint(0)) &&
|
||||
PX4_ISFINITE(_position_setpoint(1))) {
|
||||
// Use position setpoints to generate velocity setpoints
|
||||
|
||||
// Get various path specific vectors
|
||||
Vector3f pos_traj;
|
||||
pos_traj(0) = _trajectory[0].getCurrentPosition();
|
||||
pos_traj(1) = _trajectory[1].getCurrentPosition();
|
||||
pos_traj(2) = _trajectory[2].getCurrentPosition();
|
||||
Vector2f pos_traj_to_dest_xy = (_position_setpoint - pos_traj).xy();
|
||||
Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero());
|
||||
|
||||
const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get();
|
||||
|
||||
// If the drone has to change altitude, stop at the waypoint, otherwise fly through
|
||||
const float arrival_speed = has_reached_altitude ? _getSpeedAtTarget(0.f) : 0.f;
|
||||
const Vector2f max_arrival_vel = u_pos_traj_to_dest_xy * arrival_speed;
|
||||
|
||||
Vector2f vel_abs_max_xy(_getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(0)), max_arrival_vel(0)),
|
||||
_getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(1)), max_arrival_vel(1)));
|
||||
|
||||
|
||||
const Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * _mc_cruise_speed;
|
||||
|
||||
// Constrain the norm of each component using min and max values
|
||||
Vector2f vel_sp_constrained_xy(_constrainAbs(vel_sp_xy(0), vel_abs_max_xy(0)),
|
||||
_constrainAbs(vel_sp_xy(1), vel_abs_max_xy(1)));
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
// If available, constrain the velocity using _velocity_setpoint(.)
|
||||
if (PX4_ISFINITE(_velocity_setpoint(i))) {
|
||||
_velocity_setpoint(i) = _constrainOneSide(vel_sp_constrained_xy(i), _velocity_setpoint(i));
|
||||
|
||||
} else {
|
||||
_velocity_setpoint(i) = vel_sp_constrained_xy(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(_position_setpoint(2))) {
|
||||
const float vel_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
|
||||
_param_mpc_z_traj_p.get(); // Generate a velocity target for the trajectory using a simple P loop
|
||||
|
||||
// If available, constrain the velocity using _velocity_setpoint(.)
|
||||
if (PX4_ISFINITE(_velocity_setpoint(2))) {
|
||||
_velocity_setpoint(2) = _constrainOneSide(vel_sp_z, _velocity_setpoint(2));
|
||||
|
||||
} else {
|
||||
_velocity_setpoint(2) = vel_sp_z;
|
||||
}
|
||||
|
||||
_want_takeoff = _velocity_setpoint(2) < -0.3f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
|
||||
{
|
||||
// Update the constraints of the trajectories
|
||||
_trajectory[0].setMaxAccel(_param_mpc_acc_hor.get()); // TODO : Should be computed using heading
|
||||
_trajectory[1].setMaxAccel(_param_mpc_acc_hor.get());
|
||||
_trajectory[0].setMaxVel(_param_mpc_xy_vel_max.get());
|
||||
_trajectory[1].setMaxVel(_param_mpc_xy_vel_max.get());
|
||||
_trajectory[0].setMaxJerk(_param_mpc_jerk_auto.get()); // TODO : Should be computed using heading
|
||||
_trajectory[1].setMaxJerk(_param_mpc_jerk_auto.get());
|
||||
_trajectory[2].setMaxJerk(_param_mpc_jerk_auto.get());
|
||||
|
||||
if (_velocity_setpoint(2) < 0.f) { // up
|
||||
_trajectory[2].setMaxAccel(_param_mpc_acc_up_max.get());
|
||||
_trajectory[2].setMaxVel(_param_mpc_z_vel_max_up.get());
|
||||
|
||||
} else { // down
|
||||
_trajectory[2].setMaxAccel(_param_mpc_acc_down_max.get());
|
||||
_trajectory[2].setMaxVel(_param_mpc_z_vel_max_dn.get());
|
||||
}
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_generateTrajectory()
|
||||
{
|
||||
if (!PX4_ISFINITE(_velocity_setpoint(0)) || !PX4_ISFINITE(_velocity_setpoint(1))
|
||||
|| !PX4_ISFINITE(_velocity_setpoint(2))) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Slow down the trajectory by decreasing the integration time based on the position error.
|
||||
* This is only performed when the drone is behind the trajectory
|
||||
*/
|
||||
Vector2f position_trajectory_xy(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition());
|
||||
Vector2f position_xy(_position);
|
||||
Vector2f vel_traj_xy(_trajectory[0].getCurrentVelocity(), _trajectory[1].getCurrentVelocity());
|
||||
Vector2f drone_to_trajectory_xy(position_trajectory_xy - position_xy);
|
||||
float position_error = drone_to_trajectory_xy.length();
|
||||
|
||||
float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f);
|
||||
|
||||
// Don't stretch time if the drone is ahead of the position setpoint
|
||||
if (drone_to_trajectory_xy.dot(vel_traj_xy) < 0.f) {
|
||||
time_stretch = 1.f;
|
||||
}
|
||||
|
||||
Vector3f jerk_sp_smooth;
|
||||
Vector3f accel_sp_smooth;
|
||||
Vector3f vel_sp_smooth;
|
||||
Vector3f pos_sp_smooth;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_trajectory[i].updateTraj(_deltatime, time_stretch);
|
||||
jerk_sp_smooth(i) = _trajectory[i].getCurrentJerk();
|
||||
accel_sp_smooth(i) = _trajectory[i].getCurrentAcceleration();
|
||||
vel_sp_smooth(i) = _trajectory[i].getCurrentVelocity();
|
||||
pos_sp_smooth(i) = _trajectory[i].getCurrentPosition();
|
||||
}
|
||||
|
||||
_updateTrajConstraints();
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_trajectory[i].updateDurations(_velocity_setpoint(i));
|
||||
}
|
||||
|
||||
VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only
|
||||
|
||||
_jerk_setpoint = jerk_sp_smooth;
|
||||
_acceleration_setpoint = accel_sp_smooth;
|
||||
_velocity_setpoint = vel_sp_smooth;
|
||||
_position_setpoint = pos_sp_smooth;
|
||||
}
|
||||
Reference in New Issue
Block a user