mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 12:01:36 +08:00
Flight Tasks: CMake/folder restructure (no code change)
put flight tasks in separate folder and create a lib for each goal is to select tasks for targets (e.g. because of flash issue)
This commit is contained in:
ChristophTobler
committed by
Dennis Mannhart
Dennis Mannhart
parent
9551dcb497
commit
b68fd5e0a7
@@ -0,0 +1,39 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(flight_tasks_auto_line
|
||||
FlightTaskAutoLine.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(flight_tasks_auto_line PUBLIC flight_tasks_auto)
|
||||
target_include_directories(flight_tasks_auto_line PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@@ -0,0 +1,249 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "FlightTaskAutoLine.hpp"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
static constexpr float SIGMA_NORM = 0.001f;
|
||||
|
||||
void FlightTaskAutoLine::_generateSetpoints()
|
||||
{
|
||||
if (!PX4_ISFINITE(_yaw_setpoint)) {
|
||||
// no valid heading -> set heading along track
|
||||
_generateHeadingAlongTrack();
|
||||
}
|
||||
|
||||
_generateAltitudeSetpoints();
|
||||
_generateXYsetpoints();
|
||||
}
|
||||
|
||||
void FlightTaskAutoLine::_generateHeadingAlongTrack()
|
||||
{
|
||||
Vector2f prev_to_dest = Vector2f(&(_target - _prev_wp)(0));
|
||||
|
||||
if (!_compute_heading_from_2D_vector(_yaw_setpoint, prev_to_dest)) {
|
||||
// heading could not be computed. best we can do is to set heading
|
||||
// to current yaw
|
||||
_yaw_setpoint = _yaw;
|
||||
}
|
||||
}
|
||||
|
||||
void FlightTaskAutoLine::_generateXYsetpoints()
|
||||
{
|
||||
Vector2f pos_sp_to_dest = Vector2f(&(_target - _position_setpoint)(0));
|
||||
const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < NAV_ACC_RAD.get();
|
||||
|
||||
if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < NAV_ACC_RAD.get()) ||
|
||||
(!has_reached_altitude && pos_sp_to_dest.length() < NAV_ACC_RAD.get())) {
|
||||
|
||||
// Vehicle reached target in xy and no passing required. Lock position */
|
||||
_position_setpoint(0) = _target(0);
|
||||
_position_setpoint(1) = _target(1);
|
||||
_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
|
||||
|
||||
} else {
|
||||
|
||||
// Get various path specific vectors. */
|
||||
Vector2f u_prev_to_dest = Vector2f(&(_target - _prev_wp)(0)).unit_or_zero();
|
||||
Vector2f prev_to_pos(&(_position - _prev_wp)(0));
|
||||
Vector2f closest_pt = Vector2f(&_prev_wp(0)) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
|
||||
Vector2f closest_to_dest = Vector2f(&(_target - _position)(0));
|
||||
Vector2f prev_to_dest = Vector2f(&(_target - _prev_wp)(0));
|
||||
float speed_sp_track = _mc_cruise_speed;
|
||||
float speed_sp_prev_track = math::max(Vector2f(&_velocity_setpoint(0)) * u_prev_to_dest, 0.0f);
|
||||
|
||||
// Distance to target when brake should occur. The assumption is made that
|
||||
// 1.5 * cruising speed is enough to break.
|
||||
float target_threshold = 1.5f * _mc_cruise_speed;
|
||||
float speed_threshold = _mc_cruise_speed;
|
||||
const float threshold_max = target_threshold;
|
||||
|
||||
if (target_threshold > 0.5f * prev_to_dest.length()) {
|
||||
// Target threshold cannot be more than distance from previous to target
|
||||
target_threshold = 0.5f * prev_to_dest.length();
|
||||
}
|
||||
|
||||
// Compute maximum speed at target threshold */
|
||||
if (threshold_max > NAV_ACC_RAD.get()) {
|
||||
float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - NAV_ACC_RAD.get());
|
||||
speed_threshold = m * (target_threshold - NAV_ACC_RAD.get()) + _speed_at_target; // speed at transition
|
||||
}
|
||||
|
||||
// Either accelerate or decelerate
|
||||
if (closest_to_dest.length() < target_threshold) {
|
||||
|
||||
// Vehicle is close to destination. Start to decelerate
|
||||
|
||||
if (!has_reached_altitude) {
|
||||
// Altitude is not reached yet. Vehicle has to stop first before proceeding
|
||||
_speed_at_target = 0.0f;
|
||||
}
|
||||
|
||||
float acceptance_radius = NAV_ACC_RAD.get();
|
||||
|
||||
if (_speed_at_target < 0.01f) {
|
||||
// If vehicle wants to stop at the target, then set acceptance radius to zero as well.
|
||||
acceptance_radius = 0.0f;
|
||||
}
|
||||
|
||||
if ((target_threshold - acceptance_radius) >= SIGMA_NORM) {
|
||||
|
||||
// Slow down depending on distance to target minus acceptance radius.
|
||||
float m = (speed_threshold - _speed_at_target) / (target_threshold - acceptance_radius);
|
||||
speed_sp_track = m * (closest_to_dest.length() - acceptance_radius) + _speed_at_target; // speed at transition
|
||||
|
||||
} else {
|
||||
speed_sp_track = _speed_at_target;
|
||||
}
|
||||
|
||||
// If we are close to target and the previous speed setpoint along track was smaller than
|
||||
// current speed setpoint along track, then take over the previous one.
|
||||
// This ensures smoothness since we anyway want to slow down.
|
||||
if ((speed_sp_prev_track < speed_sp_track)
|
||||
&& (speed_sp_track * speed_sp_prev_track > 0.0f)
|
||||
&& (speed_sp_prev_track > _speed_at_target)) {
|
||||
speed_sp_track = speed_sp_prev_track;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
// Vehicle is still far from destination. Accelerate or keep maximum target speed.
|
||||
float acc_track = (speed_sp_track - speed_sp_prev_track) / _deltatime;
|
||||
|
||||
float yaw_diff = 0.0f;
|
||||
|
||||
if (PX4_ISFINITE(_yaw_setpoint)) {
|
||||
yaw_diff = wrap_pi(_yaw_setpoint - _yaw);
|
||||
}
|
||||
|
||||
// If yaw offset is large, only accelerate with 0.5 m/s^2.
|
||||
float acc_max = (fabsf(yaw_diff) > math::radians(MIS_YAW_ERR.get())) ? 0.5f : MPC_ACC_HOR.get();
|
||||
|
||||
if (acc_track > acc_max) {
|
||||
// accelerate towards target
|
||||
speed_sp_track = acc_max * _deltatime + speed_sp_prev_track;
|
||||
}
|
||||
}
|
||||
|
||||
speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed);
|
||||
|
||||
_position_setpoint(0) = closest_pt(0);
|
||||
_position_setpoint(1) = closest_pt(1);
|
||||
Vector2f velocity_sp_xy = u_prev_to_dest * speed_sp_track;
|
||||
_velocity_setpoint(0) = velocity_sp_xy(0);
|
||||
_velocity_setpoint(1) = velocity_sp_xy(1);
|
||||
}
|
||||
}
|
||||
|
||||
void FlightTaskAutoLine::_generateAltitudeSetpoints()
|
||||
{
|
||||
// Total distance between previous and target set-point.
|
||||
const float dist = fabsf(_target(2) - _prev_wp(2));
|
||||
|
||||
// If target has not been reached, then compute set-point depending on maximum velocity.
|
||||
if ((dist > SIGMA_NORM) && (fabsf(_position(2) - _target(2)) > 0.1f)) {
|
||||
|
||||
// get various distances */
|
||||
const float dist_to_prev = fabsf(_position(2) - _prev_wp(2));
|
||||
const float dist_to_target = fabsf(_target(2) - _position(2));
|
||||
|
||||
// check sign
|
||||
const bool flying_upward = _target(2) < _position(2);
|
||||
|
||||
// limit vertical downwards speed (positive z) close to ground
|
||||
// for now we use the altitude above home and assume that we want to land at same height as we took off
|
||||
float vel_limit = math::gradual(_alt_above_ground,
|
||||
MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
|
||||
MPC_LAND_SPEED.get(), _constraints.speed_down);
|
||||
|
||||
// Speed at threshold is by default maximum speed. Threshold defines
|
||||
// the point in z at which vehicle slows down to reach target altitude.
|
||||
float speed_sp = (flying_upward) ? _constraints.speed_up : vel_limit;
|
||||
|
||||
// Target threshold defines the distance to target(2) at which
|
||||
// the vehicle starts to slow down to approach the target smoothly.
|
||||
float target_threshold = speed_sp * 1.5f;
|
||||
|
||||
// If the total distance in z is NOT 2x distance of target_threshold, we
|
||||
// will need to adjust the final_velocity in z.
|
||||
const bool is_2_target_threshold = dist >= 2.0f * target_threshold;
|
||||
const float min_vel = 0.2f; // minimum velocity: this is needed since estimation is not perfect
|
||||
const float slope = (speed_sp - min_vel) / (target_threshold); // defines the the acceleration when slowing down */
|
||||
|
||||
if (!is_2_target_threshold) {
|
||||
// Adjust final_velocity since we are already are close to target and therefore it is not necessary to accelerate
|
||||
// upwards with full speed.
|
||||
target_threshold = dist * 0.5f;
|
||||
// get the velocity at target_threshold
|
||||
speed_sp = slope * (target_threshold) + min_vel;
|
||||
}
|
||||
|
||||
// we want to slow down
|
||||
if (dist_to_target < target_threshold) {
|
||||
|
||||
speed_sp = slope * dist_to_target + min_vel;
|
||||
|
||||
} else if (dist_to_prev < target_threshold) {
|
||||
// we want to accelerate
|
||||
|
||||
const float acc = (speed_sp - fabsf(_velocity_setpoint(2))) / _deltatime;
|
||||
const float acc_max = (flying_upward) ? (MPC_ACC_UP_MAX.get() * 0.5f) : (MPC_ACC_DOWN_MAX.get() * 0.5f);
|
||||
|
||||
if (acc > acc_max) {
|
||||
speed_sp = acc_max * _deltatime + fabsf(_velocity_setpoint(2));
|
||||
}
|
||||
}
|
||||
|
||||
// make sure vel_sp_z is always positive
|
||||
if (speed_sp < 0.0f) {
|
||||
PX4_WARN("speed cannot be smaller than 0");
|
||||
speed_sp = 0.0f;
|
||||
}
|
||||
|
||||
// get the sign of vel_sp_z
|
||||
_velocity_setpoint(2) = (flying_upward) ? -speed_sp : speed_sp;
|
||||
_position_setpoint(2) = NAN; // We don't care about position setpoint
|
||||
|
||||
} else {
|
||||
|
||||
// vehicle reached desired target altitude
|
||||
_velocity_setpoint(2) = 0.0f;
|
||||
_position_setpoint(2) = _target(2);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 FlightTaskAutoLine.hpp
|
||||
*
|
||||
* Flight task for autonomous, gps driven mode. The vehicle flies
|
||||
* along a straight line in between waypoints.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "FlightTaskAutoMapper.hpp"
|
||||
|
||||
class FlightTaskAutoLine : public FlightTaskAutoMapper
|
||||
{
|
||||
public:
|
||||
FlightTaskAutoLine() = default;
|
||||
virtual ~FlightTaskAutoLine() = default;
|
||||
|
||||
protected:
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
|
||||
(ParamFloat<px4::params::MIS_YAW_ERR>) MIS_YAW_ERR, // yaw-error threshold
|
||||
(ParamFloat<px4::params::MPC_ACC_HOR>) MPC_ACC_HOR, // acceleration in flight
|
||||
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) MPC_ACC_UP_MAX,
|
||||
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) MPC_ACC_DOWN_MAX
|
||||
);
|
||||
|
||||
void _generateSetpoints() override; /**< Generate setpoints along line. */
|
||||
|
||||
void _generateHeadingAlongTrack(); /**< Generates heading along track. */
|
||||
void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */
|
||||
void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */
|
||||
};
|
||||
Reference in New Issue
Block a user