mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 20:53:35 +08:00
Auto Smooth Vel - Add AutoMapper2 and AutoLineSmoothVel flight tasks
This commit is contained in:
bresch
@@ -59,6 +59,7 @@ list(APPEND flight_tasks_all
|
||||
ManualPositionSmoothVel
|
||||
Sport
|
||||
AutoLine
|
||||
AutoLineSmoothVel
|
||||
AutoFollowMe
|
||||
Offboard
|
||||
Failsafe
|
||||
|
||||
@@ -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(FlightTaskAutoLineSmoothVel
|
||||
FlightTaskAutoLineSmoothVel.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(FlightTaskAutoLineSmoothVel PUBLIC FlightTaskAutoMapper2 FlightTaskUtility)
|
||||
target_include_directories(FlightTaskAutoLineSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@@ -0,0 +1,206 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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()
|
||||
{
|
||||
bool ret = FlightTaskAutoMapper2::activate();
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_trajectory[i].reset(0.f, _velocity(i), _position(i));
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_setDefaultConstraints()
|
||||
{
|
||||
FlightTaskAuto::_setDefaultConstraints();
|
||||
|
||||
_constraints.speed_xy = MPC_XY_VEL_MAX.get(); // TODO : Should be computed using heading
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_generateSetpoints()
|
||||
{
|
||||
if (!PX4_ISFINITE(_yaw_setpoint)) {
|
||||
// no valid heading -> set heading along track
|
||||
// TODO: Generate heading along trajectory velocity vector
|
||||
_generateHeadingAlongTrack();
|
||||
}
|
||||
|
||||
_prepareSetpoints();
|
||||
_generateTrajectory();
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_generateHeadingAlongTrack()
|
||||
{
|
||||
Vector2f prev_to_dest(_target - _prev_wp);
|
||||
_compute_heading_from_2D_vector(_yaw_setpoint, prev_to_dest);
|
||||
|
||||
}
|
||||
|
||||
/* Constrain some value vith a constrain depending on the sign of the constrain
|
||||
* 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
|
||||
*/
|
||||
inline float FlightTaskAutoLineSmoothVel::constrain_one_side(float val, float constrain)
|
||||
{
|
||||
const float min = (constrain < FLT_EPSILON) ? constrain : 0.f;
|
||||
const float max = (constrain > FLT_EPSILON) ? constrain : 0.f;
|
||||
|
||||
return math::constrain(val, min, max);
|
||||
}
|
||||
|
||||
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.
|
||||
|
||||
if (PX4_ISFINITE(_position_setpoint(0)) &&
|
||||
PX4_ISFINITE(_position_setpoint(1))) {
|
||||
// Use position setpoints to generate velocity setpoints
|
||||
|
||||
// Get various path specific vectors. */
|
||||
Vector2f pos_traj;
|
||||
pos_traj(0) = _trajectory[0].getCurrentPosition();
|
||||
pos_traj(1) = _trajectory[1].getCurrentPosition();
|
||||
Vector2f pos_sp_xy(_position_setpoint);
|
||||
Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj);
|
||||
Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero();
|
||||
Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp));
|
||||
Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
|
||||
Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero());
|
||||
|
||||
float speed_sp_track = _mc_cruise_speed;
|
||||
|
||||
speed_sp_track = Vector2f(pos_traj_to_dest).length() * 0.3f;
|
||||
speed_sp_track = math::constrain(speed_sp_track, 0.0f, MPC_XY_CRUISE.get());
|
||||
Vector2f velocity_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track;
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
// If available, constrain the velocity using _velocity_setpoint(.)
|
||||
if (PX4_ISFINITE(_velocity_setpoint(i))) {
|
||||
_velocity_setpoint(i) = constrain_one_side(velocity_sp_xy(i), _velocity_setpoint(i));
|
||||
|
||||
} else {
|
||||
_velocity_setpoint(i) = velocity_sp_xy(i);
|
||||
}
|
||||
|
||||
_velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) *
|
||||
0.3f; // Along-track setpoint + cross-track P controller
|
||||
}
|
||||
|
||||
} else if (!PX4_ISFINITE(_velocity_setpoint(0)) &&
|
||||
!PX4_ISFINITE(_velocity_setpoint(1))) {
|
||||
// No position nor velocity setpoints available, set the velocity targer to zero
|
||||
|
||||
_velocity_setpoint(0) = 0.f;
|
||||
_velocity_setpoint(1) = 0.f;
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(_position_setpoint(2))) {
|
||||
const float velocity_sp_z = (_position_setpoint(2) - _trajectory[2].getCurrentPosition()) *
|
||||
0.3f; // 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) = constrain_one_side(velocity_sp_z, _velocity_setpoint(2));
|
||||
|
||||
} else {
|
||||
_velocity_setpoint(2) = velocity_sp_z;
|
||||
}
|
||||
|
||||
} else if (!PX4_ISFINITE(_velocity_setpoint(2))) {
|
||||
// No position nor velocity setpoints available, set the velocity targer to zero
|
||||
_velocity_setpoint(2) = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
void FlightTaskAutoLineSmoothVel::_generateTrajectory()
|
||||
{
|
||||
// Update the constraints of the trajectories
|
||||
_trajectory[0].setMaxAccel(MPC_ACC_HOR_MAX.get()); // TODO : Should be computed using heading
|
||||
_trajectory[1].setMaxAccel(MPC_ACC_HOR_MAX.get());
|
||||
_trajectory[0].setMaxVel(_constraints.speed_xy);
|
||||
_trajectory[1].setMaxVel(_constraints.speed_xy);
|
||||
_trajectory[0].setMaxJerk(MPC_JERK_MIN.get()); // TODO : Should be computed using heading
|
||||
_trajectory[1].setMaxJerk(MPC_JERK_MIN.get());
|
||||
_trajectory[2].setMaxJerk(MPC_JERK_MIN.get());
|
||||
|
||||
if (_velocity_setpoint(2) < 0.f) { // up
|
||||
_trajectory[2].setMaxAccel(MPC_ACC_UP_MAX.get());
|
||||
_trajectory[2].setMaxVel(MPC_Z_VEL_MAX_UP.get());
|
||||
|
||||
} else { // down
|
||||
_trajectory[2].setMaxAccel(MPC_ACC_DOWN_MAX.get());
|
||||
_trajectory[2].setMaxVel(MPC_Z_VEL_MAX_DN.get());
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_trajectory[i].updateDurations(_deltatime, _velocity_setpoint(i));
|
||||
}
|
||||
|
||||
VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only
|
||||
|
||||
/* 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 accel_sp_smooth; // Dummy variable
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
_trajectory[i].integrate(_deltatime * time_stretch, accel_sp_smooth(i), _velocity_setpoint(i), _position_setpoint(i));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,73 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 FlightTaskAutoLineSmoothVel.hpp
|
||||
*
|
||||
* Flight task for autonomous, gps driven mode. The vehicle flies
|
||||
* along a straight line in between waypoints.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "FlightTaskAutoMapper2.hpp"
|
||||
#include "VelocitySmoothing.hpp"
|
||||
|
||||
class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2
|
||||
{
|
||||
public:
|
||||
FlightTaskAutoLineSmoothVel() = default;
|
||||
virtual ~FlightTaskAutoLineSmoothVel() = default;
|
||||
|
||||
bool activate() override;
|
||||
|
||||
protected:
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper2,
|
||||
(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,
|
||||
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) MPC_ACC_HOR_MAX,
|
||||
(ParamFloat<px4::params::MPC_JERK_MIN>) MPC_JERK_MIN
|
||||
);
|
||||
|
||||
void _generateSetpoints() override; /**< Generate setpoints along line. */
|
||||
void _setDefaultConstraints() override;
|
||||
|
||||
inline float constrain_one_side(float val, float constrain);
|
||||
void _generateHeadingAlongTrack(); /**< Generates heading along track. */
|
||||
void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
|
||||
void _generateTrajectory();
|
||||
VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
|
||||
};
|
||||
@@ -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(FlightTaskAutoMapper2
|
||||
FlightTaskAutoMapper2.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(FlightTaskAutoMapper2 PUBLIC FlightTaskAuto)
|
||||
target_include_directories(FlightTaskAutoMapper2 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@@ -0,0 +1,189 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 FlightAutoMapper2.cpp
|
||||
*/
|
||||
|
||||
#include "FlightTaskAutoMapper2.hpp"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
bool FlightTaskAutoMapper2::activate()
|
||||
{
|
||||
bool ret = FlightTaskAuto::activate();
|
||||
_reset();
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool FlightTaskAutoMapper2::update()
|
||||
{
|
||||
// always reset constraints because they might change depending on the type
|
||||
_setDefaultConstraints();
|
||||
|
||||
_updateAltitudeAboveGround();
|
||||
|
||||
// The only time a thrust set-point is sent out is during
|
||||
// idle. Hence, reset thrust set-point to NAN in case the
|
||||
// vehicle exits idle.
|
||||
|
||||
if (_type_previous == WaypointType::idle) {
|
||||
_thrust_setpoint = Vector3f(NAN, NAN, NAN);
|
||||
}
|
||||
|
||||
switch (_type) {
|
||||
case WaypointType::idle:
|
||||
_prepareIdleSetpoints();
|
||||
break;
|
||||
|
||||
case WaypointType::land:
|
||||
_prepareLandSetpoints();
|
||||
break;
|
||||
|
||||
case WaypointType::loiter:
|
||||
|
||||
/* fallthrought */
|
||||
case WaypointType::position:
|
||||
_preparePositionSetpoints();
|
||||
break;
|
||||
|
||||
case WaypointType::takeoff:
|
||||
_prepareTakeoffSetpoints();
|
||||
break;
|
||||
|
||||
case WaypointType::velocity:
|
||||
_prepareVelocitySetpoints();
|
||||
break;
|
||||
|
||||
default:
|
||||
_preparePositionSetpoints();
|
||||
break;
|
||||
}
|
||||
|
||||
_generateSetpoints();
|
||||
|
||||
// during mission and reposition, raise the landing gears but only
|
||||
// if altitude is high enough
|
||||
if (_highEnoughForLandingGear()) {
|
||||
_constraints.landing_gear = vehicle_constraints_s::GEAR_UP;
|
||||
}
|
||||
|
||||
// update previous type
|
||||
_type_previous = _type;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void FlightTaskAutoMapper2::_reset()
|
||||
{
|
||||
// Set setpoints equal current state.
|
||||
_velocity_setpoint = _velocity;
|
||||
_position_setpoint = _position;
|
||||
_speed_at_target = 0.0f;
|
||||
}
|
||||
|
||||
void FlightTaskAutoMapper2::_prepareIdleSetpoints()
|
||||
{
|
||||
// Send zero thrust setpoint
|
||||
_position_setpoint = Vector3f(NAN, NAN, NAN); // Don't require any position/velocity setpoints
|
||||
_velocity_setpoint = Vector3f(NAN, NAN, NAN);
|
||||
_thrust_setpoint.zero();
|
||||
}
|
||||
|
||||
void FlightTaskAutoMapper2::_prepareLandSetpoints()
|
||||
{
|
||||
// Keep xy-position and go down with landspeed
|
||||
_position_setpoint = Vector3f(_target(0), _target(1), NAN);
|
||||
const float speed_lnd = (_alt_above_ground > MPC_LAND_ALT1.get()) ? _constraints.speed_down : MPC_LAND_SPEED.get();
|
||||
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, speed_lnd));
|
||||
|
||||
// set constraints
|
||||
_constraints.tilt = MPC_TILTMAX_LND.get();
|
||||
_constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
|
||||
}
|
||||
|
||||
void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
|
||||
{
|
||||
// Takeoff is completely defined by target position
|
||||
_position_setpoint = _target;
|
||||
const float speed_tko = (_alt_above_ground > MPC_LAND_ALT1.get()) ? _constraints.speed_up : MPC_TKO_SPEED.get();
|
||||
_velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed
|
||||
|
||||
_constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
|
||||
}
|
||||
|
||||
void FlightTaskAutoMapper2::_prepareVelocitySetpoints()
|
||||
{
|
||||
// XY Velocity waypoint
|
||||
// TODO : Rewiew that. What is the expected behavior?
|
||||
_position_setpoint = Vector3f(NAN, NAN, _position(2));
|
||||
Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
|
||||
_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
|
||||
}
|
||||
|
||||
void FlightTaskAutoMapper2::_preparePositionSetpoints()
|
||||
{
|
||||
// Simple waypoint navigation: go to xyz target, with standard limitations
|
||||
_position_setpoint = _target;
|
||||
_velocity_setpoint = Vector3f(NAN, NAN, NAN); // No special velocity limitations
|
||||
}
|
||||
|
||||
void FlightTaskAutoMapper2::_updateAltitudeAboveGround()
|
||||
{
|
||||
// Altitude above ground is by default just the negation of the current local position in D-direction.
|
||||
_alt_above_ground = -_position(2);
|
||||
|
||||
if (PX4_ISFINITE(_dist_to_bottom)) {
|
||||
// We have a valid distance to ground measurement
|
||||
_alt_above_ground = _dist_to_bottom;
|
||||
|
||||
} else if (_sub_home_position->get().valid_alt) {
|
||||
// if home position is set, then altitude above ground is relative to the home position
|
||||
_alt_above_ground = -_position(2) + _sub_home_position->get().z;
|
||||
}
|
||||
}
|
||||
|
||||
void FlightTaskAutoMapper2::updateParams()
|
||||
{
|
||||
FlightTaskAuto::updateParams();
|
||||
|
||||
// make sure that alt1 is above alt2
|
||||
MPC_LAND_ALT1.set(math::max(MPC_LAND_ALT1.get(), MPC_LAND_ALT2.get()));
|
||||
}
|
||||
|
||||
bool FlightTaskAutoMapper2::_highEnoughForLandingGear()
|
||||
{
|
||||
// return true if altitude is above two meters
|
||||
return _alt_above_ground > 2.0f;
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 FlightTaskAutoMapper2.hpp
|
||||
*
|
||||
* Abstract Flight task which generates local setpoints
|
||||
* based on the triplet type.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "FlightTaskAuto.hpp"
|
||||
|
||||
class FlightTaskAutoMapper2 : public FlightTaskAuto
|
||||
{
|
||||
public:
|
||||
FlightTaskAutoMapper2() = default;
|
||||
virtual ~FlightTaskAutoMapper2() = default;
|
||||
bool activate() override;
|
||||
bool update() override;
|
||||
|
||||
protected:
|
||||
|
||||
float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
|
||||
(ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED,
|
||||
(ParamFloat<px4::params::MPC_TILTMAX_LND>) MPC_TILTMAX_LND,
|
||||
(ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, // altitude at which speed limit downwards reaches maximum speed
|
||||
(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed
|
||||
(ParamFloat<px4::params::MPC_TKO_SPEED>) MPC_TKO_SPEED
|
||||
);
|
||||
|
||||
virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */
|
||||
|
||||
void _prepareIdleSetpoints();
|
||||
void _prepareLandSetpoints();
|
||||
void _prepareVelocitySetpoints();
|
||||
void _prepareTakeoffSetpoints();
|
||||
void _preparePositionSetpoints();
|
||||
|
||||
void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */
|
||||
void updateParams() override; /**< See ModuleParam class */
|
||||
|
||||
private:
|
||||
|
||||
void _reset(); /**< Resets member variables to current vehicle state */
|
||||
WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
|
||||
bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
|
||||
};
|
||||
@@ -37,6 +37,7 @@ add_subdirectory(Utility)
|
||||
add_subdirectory(Manual)
|
||||
add_subdirectory(Auto)
|
||||
add_subdirectory(AutoMapper)
|
||||
add_subdirectory(AutoMapper2)
|
||||
|
||||
# add all additional flight tasks
|
||||
foreach(task ${flight_tasks_all})
|
||||
|
||||
@@ -885,7 +885,22 @@ MulticopterPositionControl::start_flight_task()
|
||||
|
||||
} else if (_control_mode.flag_control_auto_enabled) {
|
||||
// Auto relate maneuvers
|
||||
int error = _flight_tasks.switchTask(FlightTaskIndex::AutoLine);
|
||||
int error = 0;
|
||||
switch (MPC_POS_MODE.get()) {
|
||||
case 0:
|
||||
case 1:
|
||||
case 2:
|
||||
error = _flight_tasks.switchTask(FlightTaskIndex::AutoLine);
|
||||
break;
|
||||
|
||||
case 3:
|
||||
error = _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel);
|
||||
break;
|
||||
|
||||
default:
|
||||
error = _flight_tasks.switchTask(FlightTaskIndex::AutoLine);
|
||||
break;
|
||||
}
|
||||
|
||||
if (error != 0) {
|
||||
if (prev_failure_count == 0) {
|
||||
|
||||
Reference in New Issue
Block a user