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

FlightTasks: remove deprecated AutoLine mission task

Browse Source
This commit is contained in:
Matthias Grob
2019-12-18 15:52:41 +01:00
committed by Mathieu Bresciani
parent bb410ca0db
commit 1484dfed6a
18 changed files with 96 additions and 856 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/CMakeLists.txt
-1
View File
@@ -58,7 +58,6 @@ list(APPEND flight_tasks_all
ManualPositionSmooth
ManualPositionSmoothVel
Sport
AutoLine
AutoLineSmoothVel
AutoFollowMe
Offboard
src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp
-1
View File
@@ -114,7 +114,6 @@ protected:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_CRUISE_90>) _param_mpc_cruise_90, // speed at corner when angle is 90 degrees move to line
(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
(ParamInt<px4::params::MPC_YAW_MODE>) _param_mpc_yaw_mode, // defines how heading is executed,
src/lib/flight_tasks/tasks/AutoLine/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# 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(FlightTaskAutoLine
FlightTaskAutoLine.cpp
)
target_link_libraries(FlightTaskAutoLine PUBLIC FlightTaskAutoMapper)
target_include_directories(FlightTaskAutoLine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp
-275
View File
@@ -1,275 +0,0 @@
/****************************************************************************
*
* 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();
}
if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
// Wait for the yaw setpoint to be aligned
if (!_position_locked) {
_velocity_setpoint.setAll(0.f);
_position_setpoint = _position;
_position_locked = true;
}
} else {
_position_locked = false;
_generateAltitudeSetpoints();
_generateXYsetpoints();
}
}
void FlightTaskAutoLine::_setSpeedAtTarget()
{
// angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0
float angle = 2.0f; // minimum angle
_speed_at_target = 0.0f;
if (Vector2f(&(_target - _next_wp)(0)).length() > 0.001f &&
(Vector2f(&(_target - _prev_wp)(0)).length() > _target_acceptance_radius)) {
// angle = cosf(x) + 1.0f
angle =
Vector2f(&(_target - _prev_wp)(0)).unit_or_zero()
* Vector2f(&(_target - _next_wp)(0)).unit_or_zero()
+ 1.0f;
_speed_at_target = math::expontialFromLimits(angle, 0.0f, _param_mpc_cruise_90.get(), _mc_cruise_speed);
}
}
void FlightTaskAutoLine::_generateHeadingAlongTrack()
{
Vector2f prev_to_dest(_target - _prev_wp);
_compute_heading_from_2D_vector(_yaw_setpoint, prev_to_dest);
}
void FlightTaskAutoLine::_generateXYsetpoints()
{
_setSpeedAtTarget();
Vector2f pos_sp_to_dest(_target - _position_setpoint);
const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _param_nav_mc_alt_rad.get();
if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _target_acceptance_radius) ||
(!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) {
// 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 {
// Vehicle needs to pass waypoint
// Ensure that vehicle never gets stuck because of too low target-speed
_speed_at_target = math::max(_speed_at_target, 0.5f);
// Get various path specific vectors. */
Vector2f u_prev_to_dest = Vector2f(_target - _prev_wp).unit_or_zero();
Vector2f prev_to_pos(_position - _prev_wp);
Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest);
Vector2f closest_to_dest(_target - _position);
Vector2f prev_to_dest(_target - _prev_wp);
float speed_sp_track = _mc_cruise_speed;
float speed_sp_prev_track = math::max(Vector2f(_velocity_setpoint) * 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 > _target_acceptance_radius) {
float m = (_mc_cruise_speed - _speed_at_target) / (threshold_max - _target_acceptance_radius);
speed_threshold = m * (target_threshold - _target_acceptance_radius) + _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 = _target_acceptance_radius;
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;
// If yaw offset is large, only accelerate with 0.5 m/s^2.
float acc_max = (_yaw_sp_aligned) ? _param_mpc_acc_hor.get() : 0.5f;
if (acc_track > acc_max) {
// accelerate towards target
speed_sp_track = acc_max * _deltatime + speed_sp_prev_track;
}
// ensure that desired speed does not exceed speed at threshold
speed_sp_track = math::min(speed_threshold, speed_sp_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(_dist_to_ground,
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_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) ? (_param_mpc_acc_up_max.get() * 0.5f) : (_param_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);
}
}
src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.hpp
-70
View File
@@ -1,70 +0,0 @@
/****************************************************************************
*
* 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:
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. */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
);
private:
void _setSpeedAtTarget(); /**< Sets desiered speed at target */
float _speed_at_target{0.0f};
bool _position_locked{false};
};
src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt
+1 -1
View File
@@ -35,7 +35,7 @@ px4_add_library(FlightTaskAutoLineSmoothVel
FlightTaskAutoLineSmoothVel.cpp
)
target_link_libraries(FlightTaskAutoLineSmoothVel PUBLIC FlightTaskAutoMapper2 FlightTaskUtility)
target_link_libraries(FlightTaskAutoLineSmoothVel PUBLIC FlightTaskAutoMapper FlightTaskUtility)
target_include_directories(FlightTaskAutoLineSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+1 -1
View File
@@ -43,7 +43,7 @@ using namespace matrix;
bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s last_setpoint)
{
bool ret = FlightTaskAutoMapper2::activate(last_setpoint);
bool ret = FlightTaskAutoMapper::activate(last_setpoint);
checkSetpoints(last_setpoint);
const Vector3f vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz);
src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+3 -3
View File
@@ -40,10 +40,10 @@
#pragma once
#include "FlightTaskAutoMapper2.hpp"
#include "FlightTaskAutoMapper.hpp"
#include "VelocitySmoothing.hpp"
class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2
class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper
{
public:
FlightTaskAutoLineSmoothVel() = default;
@@ -84,7 +84,7 @@ protected:
VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper2,
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
+80 -32
View File
@@ -52,14 +52,6 @@ bool FlightTaskAutoMapper::update()
// always reset constraints because they might change depending on the type
_setDefaultConstraints();
bool follow_line = _type == WaypointType::loiter || _type == WaypointType::position;
bool follow_line_prev = _type_previous == WaypointType::loiter || _type_previous == WaypointType::position;
// 1st time that vehicle starts to follow line. Reset all setpoints to current vehicle state.
if (follow_line && !follow_line_prev) {
_reset();
}
// 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.
@@ -71,23 +63,36 @@ bool FlightTaskAutoMapper::update()
// during mission and reposition, raise the landing gears but only
// if altitude is high enough
if (_highEnoughForLandingGear()) {
_gear.landing_gear = _mission_gear;
_gear.landing_gear = landing_gear_s::GEAR_UP;
}
if (_type == WaypointType::idle) {
_generateIdleSetpoints();
switch (_type) {
case WaypointType::idle:
_prepareIdleSetpoints();
break;
} else if (_type == WaypointType::land) {
_generateLandSetpoints();
case WaypointType::land:
_prepareLandSetpoints();
break;
} else if (follow_line) {
_generateSetpoints();
case WaypointType::loiter:
} else if (_type == WaypointType::takeoff) {
_generateTakeoffSetpoints();
/* fallthrought */
case WaypointType::position:
_preparePositionSetpoints();
break;
} else if (_type == WaypointType::velocity) {
_generateVelocitySetpoints();
case WaypointType::takeoff:
_prepareTakeoffSetpoints();
break;
case WaypointType::velocity:
_prepareVelocitySetpoints();
break;
default:
_preparePositionSetpoints();
break;
}
if (_param_com_obs_avoid.get()) {
@@ -96,6 +101,9 @@ bool FlightTaskAutoMapper::update()
_yawspeed_setpoint);
}
_generateSetpoints();
// update previous type
_type_previous = _type;
@@ -109,7 +117,7 @@ void FlightTaskAutoMapper::_reset()
_position_setpoint = _position;
}
void FlightTaskAutoMapper::_generateIdleSetpoints()
void FlightTaskAutoMapper::_prepareIdleSetpoints()
{
// Send zero thrust setpoint
_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
@@ -117,39 +125,46 @@ void FlightTaskAutoMapper::_generateIdleSetpoints()
_thrust_setpoint.zero();
}
void FlightTaskAutoMapper::_generateLandSetpoints()
void FlightTaskAutoMapper::_prepareLandSetpoints()
{
float land_speed = _getLandSpeed();
// Keep xy-position and go down with landspeed
_position_setpoint = Vector3f(_target(0), _target(1), NAN);
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _param_mpc_land_speed.get()));
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed));
// set constraints
_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
_constraints.speed_down = _param_mpc_land_speed.get();
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper::_generateTakeoffSetpoints()
void FlightTaskAutoMapper::_prepareTakeoffSetpoints()
{
// Takeoff is completely defined by target position
_position_setpoint = _target;
_velocity_setpoint = Vector3f(NAN, NAN, NAN);
// limit vertical speed during takeoff
_constraints.speed_up = math::gradual(_dist_to_ground, _param_mpc_land_alt2.get(),
_param_mpc_land_alt1.get(), _param_mpc_tko_speed.get(), _constraints.speed_up);
const float speed_tko = (_dist_to_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up :
_param_mpc_tko_speed.get();
_velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper::_generateVelocitySetpoints()
void FlightTaskAutoMapper::_prepareVelocitySetpoints()
{
// TODO: Remove velocity force logic from navigator, since
// navigator should only send out waypoints.
// 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 FlightTaskAutoMapper::_preparePositionSetpoints()
{
// Simple waypoint navigation: go to xyz target, with standard limitations
_position_setpoint = _target;
_velocity_setpoint.setNaN(); // No special velocity limitations
}
void FlightTaskAutoMapper::updateParams()
{
FlightTaskAuto::updateParams();
@@ -163,3 +178,36 @@ bool FlightTaskAutoMapper::_highEnoughForLandingGear()
// return true if altitude is above two meters
return _dist_to_ground > 2.0f;
}
float FlightTaskAutoMapper::_getLandSpeed()
{
bool rc_assist_enabled = _param_mpc_land_rc_help.get();
bool rc_is_valid = !_sub_vehicle_status.get().rc_signal_lost;
float throttle = 0.5f;
if (rc_is_valid && rc_assist_enabled) {
throttle = _sub_manual_control_setpoint.get().z;
}
float speed = 0;
if (_dist_to_ground > _param_mpc_land_alt1.get()) {
speed = _constraints.speed_down;
} else {
const float land_speed = math::gradual(_dist_to_ground,
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_mpc_land_speed.get(), _constraints.speed_down);
const float head_room = _constraints.speed_down - land_speed;
speed = land_speed + 2 * (0.5f - throttle) * head_room;
// Allow minimum assisted land speed to be half of parameter
if (speed < land_speed * 0.5f) {
speed = land_speed * 0.5f;
}
}
return speed;
}
src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp
+7 -4
View File
@@ -54,16 +54,18 @@ protected:
virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */
void _generateIdleSetpoints();
void _generateLandSetpoints();
void _generateVelocitySetpoints();
void _generateTakeoffSetpoints();
void _prepareIdleSetpoints();
void _prepareLandSetpoints();
void _prepareVelocitySetpoints();
void _prepareTakeoffSetpoints();
void _preparePositionSetpoints();
void updateParams() override; /**< See ModuleParam class */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
(ParamFloat<px4::params::MPC_LAND_ALT1>)
_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
(ParamFloat<px4::params::MPC_LAND_ALT2>)
@@ -76,4 +78,5 @@ 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. */
float _getLandSpeed(); /**< Returns landing descent speed. */
};
src/lib/flight_tasks/tasks/AutoMapper2/CMakeLists.txt
-39
View File
@@ -1,39 +0,0 @@
############################################################################
#
# 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})
src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
-213
View File
@@ -1,213 +0,0 @@
/****************************************************************************
*
* 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(vehicle_local_position_setpoint_s last_setpoint)
{
bool ret = FlightTaskAuto::activate(last_setpoint);
_reset();
return ret;
}
bool FlightTaskAutoMapper2::update()
{
// always reset constraints because they might change depending on the type
_setDefaultConstraints();
// 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.setNaN();
}
// during mission and reposition, raise the landing gears but only
// if altitude is high enough
if (_highEnoughForLandingGear()) {
_gear.landing_gear = landing_gear_s::GEAR_UP;
}
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;
}
if (_param_com_obs_avoid.get()) {
_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
_yawspeed_setpoint);
}
_generateSetpoints();
// update previous type
_type_previous = _type;
return true;
}
void FlightTaskAutoMapper2::_reset()
{
// Set setpoints equal current state.
_velocity_setpoint = _velocity;
_position_setpoint = _position;
}
void FlightTaskAutoMapper2::_prepareIdleSetpoints()
{
// Send zero thrust setpoint
_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
_velocity_setpoint.setNaN();
_thrust_setpoint.zero();
}
void FlightTaskAutoMapper2::_prepareLandSetpoints()
{
float land_speed = _getLandSpeed();
// Keep xy-position and go down with landspeed
_position_setpoint = Vector3f(_target(0), _target(1), NAN);
_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, land_speed));
// set constraints
_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
{
// Takeoff is completely defined by target position
_position_setpoint = _target;
const float speed_tko = (_dist_to_ground > _param_mpc_land_alt1.get()) ? _constraints.speed_up :
_param_mpc_tko_speed.get();
_velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed
_gear.landing_gear = landing_gear_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::updateParams()
{
FlightTaskAuto::updateParams();
// make sure that alt1 is above alt2
_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
}
bool FlightTaskAutoMapper2::_highEnoughForLandingGear()
{
// return true if altitude is above two meters
return _dist_to_ground > 2.0f;
}
float FlightTaskAutoMapper2::_getLandSpeed()
{
bool rc_assist_enabled = _param_mpc_land_rc_help.get();
bool rc_is_valid = !_sub_vehicle_status.get().rc_signal_lost;
float throttle = 0.5f;
if (rc_is_valid && rc_assist_enabled) {
throttle = _sub_manual_control_setpoint.get().z;
}
float speed = 0;
if (_dist_to_ground > _param_mpc_land_alt1.get()) {
speed = _constraints.speed_down;
} else {
const float land_speed = math::gradual(_dist_to_ground,
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_mpc_land_speed.get(), _constraints.speed_down);
const float head_room = _constraints.speed_down - land_speed;
speed = land_speed + 2 * (0.5f - throttle) * head_room;
// Allow minimum assisted land speed to be half of parameter
if (speed < land_speed * 0.5f) {
speed = land_speed * 0.5f;
}
}
return speed;
}
src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp
-82
View File
@@ -1,82 +0,0 @@
/****************************************************************************
*
* 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(vehicle_local_position_setpoint_s last_setpoint) override;
bool update() override;
protected:
virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */
void _prepareIdleSetpoints();
void _prepareLandSetpoints();
void _prepareVelocitySetpoints();
void _prepareTakeoffSetpoints();
void _preparePositionSetpoints();
void updateParams() override; /**< See ModuleParam class */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd,
(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
(ParamFloat<px4::params::MPC_LAND_ALT1>)
_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
(ParamFloat<px4::params::MPC_LAND_ALT2>)
_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed
);
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. */
float _getLandSpeed(); /**< Returns landing descent speed. */
};
src/lib/flight_tasks/tasks/CMakeLists.txt
+1 -2
View File
@@ -37,9 +37,8 @@ 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})
add_subdirectory(${task})
endforeach()
endforeach()
src/lib/mathlib/math/Functions.hpp
-53
View File
@@ -152,57 +152,4 @@ const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low,
}
}
/*
* Exponential function of the form Y_out = a*b^X + c
*
* Y_max | *
* | *
* | *
* | *
* | *
* Y_middle | *
* | *
* Y_min | * *
* | __________________________________
* 0 1 2
*
*
* @param X in the range [0,2]
* @param Y_min minimum output at X = 2
* @param Y_mid middle output at X = 1
* @param Y_max maximum output at X = 0
*/
template<typename T>
const T expontialFromLimits(const T &X_in, const T &Y_min, const T &Y_mid, const T &Y_max)
{
const T delta = (T)0.001;
// constrain X_in to the range of 0 and 2
T X = math::constrain(X_in, (T)0, (T)2);
// If Y_mid is exactly in the middle, then just apply linear approach.
bool use_linear_approach = false;
if (((Y_max + Y_min) * (T)0.5) - Y_mid < delta) {
use_linear_approach = true;
}
T Y_out;
if (use_linear_approach) {
// Y_out = m*x+q
float slope = -(Y_max - Y_min) / (T)2.0;
Y_out = slope * X + Y_max;
} else {
// Y_out = a*b^X + c with constraints Y_max = 0, Y_middle = 1, Y_min = 2
T a = -((Y_mid - Y_max) * (Y_mid - Y_max))
/ ((T)2.0 * Y_mid - Y_max - Y_min);
T c = Y_max - a;
T b = (Y_mid - c) / a;
Y_out = a * powf(b, X) + c;
}
// Y_out needs to be in between max and min
return constrain(Y_out, Y_min, Y_max);
}
} /* namespace math */
src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp
+1 -1
View File
@@ -68,7 +68,7 @@ bool PreFlightCheck::magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_stat
if (!calibration_valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u uncalibrated", instance);
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u %u uncalibrated", instance, device_id);
}
}
src/modules/mc_pos_control/mc_pos_control_main.cpp
+1 -10
View File
@@ -169,7 +169,6 @@ private:
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
(ParamInt<px4::params::MPC_AUTO_MODE>) _param_mpc_auto_mode,
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
@@ -784,15 +783,7 @@ MulticopterPositionControl::start_flight_task()
should_disable_task = false;
FlightTaskError error = FlightTaskError::NoError;
switch (_param_mpc_auto_mode.get()) {
case 1:
error = _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel);
break;
default:
error = _flight_tasks.switchTask(FlightTaskIndex::AutoLine);
break;
}
error = _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel);
if (error != FlightTaskError::NoError) {
if (prev_failure_count == 0) {
src/modules/mc_pos_control/mc_pos_control_params.c
+1 -29
View File
@@ -259,22 +259,6 @@ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
*/
PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f);
/**
* Cruise speed when angle prev-current/current-next setpoint
* is 90 degrees. It should be lower than MPC_XY_CRUISE.
*
* Applies only in AUTO modes (includes
* also RTL / hold / etc.)
*
* @unit m/s
* @min 1.0
* @max 20.0
* @increment 1
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_CRUISE_90, 3.0f);
/**
* Maximum horizontal velocity setpoint for manual controlled mode
* If velocity setpoint larger than MPC_XY_VEL_MAX is set, then
@@ -454,8 +438,7 @@ PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f);
/**
* Maximum horizontal acceleration for auto mode and for manual mode
*
* Manual mode: Maximum deceleration for MPC_POS_MODE 1 and 2. Maximum acceleration and deceleration for MPC_POS_MODE 3.
* Auto mode: Used with MPC_AUTO_MODE 0 only. For MPC_AUTO_MODE 1, MPC_ACC_HOR is always used.
* Maximum deceleration for MPC_POS_MODE 1 and 2. Maximum acceleration and deceleration for MPC_POS_MODE 3.
*
* @unit m/s/s
* @min 2.0
@@ -569,8 +552,6 @@ PARAM_DEFINE_FLOAT(MPC_JERK_MIN, 8.0f);
* A lower value leads to smoother vehicle motions, but it also limits its
* agility.
*
* Note: This is only used in jerk-limited trajectory mode (MPC_AUTO_MODE 1)
*
* @unit m/s/s/s
* @min 5.0
* @max 80.0
@@ -733,15 +714,6 @@ PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f);
*/
PARAM_DEFINE_INT32(MPC_POS_MODE, 3);
/**
* Auto sub-mode
*
* @value 0 Default line tracking
* @value 1 Jerk-limited trajectory
* @group Multicopter Position Control
*/
PARAM_DEFINE_INT32(MPC_AUTO_MODE, 1);
/**
* Enforced delay between arming and takeoff
*