From 1484dfed6a8200781865034d812450936388efc4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 18 Dec 2019 15:52:41 +0100 Subject: [PATCH] FlightTasks: remove deprecated AutoLine mission task --- src/lib/flight_tasks/CMakeLists.txt | 1 - .../tasks/Auto/FlightTaskAuto.hpp | 1 - .../tasks/AutoLine/CMakeLists.txt | 39 --- .../tasks/AutoLine/FlightTaskAutoLine.cpp | 275 ------------------ .../tasks/AutoLine/FlightTaskAutoLine.hpp | 70 ----- .../tasks/AutoLineSmoothVel/CMakeLists.txt | 2 +- .../FlightTaskAutoLineSmoothVel.cpp | 2 +- .../FlightTaskAutoLineSmoothVel.hpp | 6 +- .../tasks/AutoMapper/FlightTaskAutoMapper.cpp | 112 +++++-- .../tasks/AutoMapper/FlightTaskAutoMapper.hpp | 11 +- .../tasks/AutoMapper2/CMakeLists.txt | 39 --- .../AutoMapper2/FlightTaskAutoMapper2.cpp | 213 -------------- .../AutoMapper2/FlightTaskAutoMapper2.hpp | 82 ------ src/lib/flight_tasks/tasks/CMakeLists.txt | 3 +- src/lib/mathlib/math/Functions.hpp | 53 ---- .../checks/magnetometerCheck.cpp | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 11 +- .../mc_pos_control/mc_pos_control_params.c | 30 +- 18 files changed, 96 insertions(+), 856 deletions(-) delete mode 100644 src/lib/flight_tasks/tasks/AutoLine/CMakeLists.txt delete mode 100644 src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp delete mode 100644 src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.hpp delete mode 100644 src/lib/flight_tasks/tasks/AutoMapper2/CMakeLists.txt delete mode 100644 src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp delete mode 100644 src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp diff --git a/src/lib/flight_tasks/CMakeLists.txt b/src/lib/flight_tasks/CMakeLists.txt index 6143dae51c..a97cf3edb4 100644 --- a/src/lib/flight_tasks/CMakeLists.txt +++ b/src/lib/flight_tasks/CMakeLists.txt @@ -58,7 +58,6 @@ list(APPEND flight_tasks_all ManualPositionSmooth ManualPositionSmoothVel Sport - AutoLine AutoLineSmoothVel AutoFollowMe Offboard diff --git a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp index 7cafd8872c..5e3ee4e879 100644 --- a/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp @@ -114,7 +114,6 @@ protected: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, (ParamFloat) _param_mpc_xy_cruise, - (ParamFloat) _param_mpc_cruise_90, // speed at corner when angle is 90 degrees move to line (ParamFloat) _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated (ParamInt) _param_mpc_yaw_mode, // defines how heading is executed, diff --git a/src/lib/flight_tasks/tasks/AutoLine/CMakeLists.txt b/src/lib/flight_tasks/tasks/AutoLine/CMakeLists.txt deleted file mode 100644 index 1621847504..0000000000 --- a/src/lib/flight_tasks/tasks/AutoLine/CMakeLists.txt +++ /dev/null @@ -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}) diff --git a/src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp b/src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp deleted file mode 100644 index 2dde9449c9..0000000000 --- a/src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.cpp +++ /dev/null @@ -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 - -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); - } -} diff --git a/src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.hpp b/src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.hpp deleted file mode 100644 index e4fc221c15..0000000000 --- a/src/lib/flight_tasks/tasks/AutoLine/FlightTaskAutoLine.hpp +++ /dev/null @@ -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) _param_mpc_acc_hor, // acceleration in flight - (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_acc_down_max - ); - - -private: - void _setSpeedAtTarget(); /**< Sets desiered speed at target */ - float _speed_at_target{0.0f}; - bool _position_locked{false}; -}; diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt index 03bca0c201..4e22acde80 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/CMakeLists.txt @@ -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}) diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index fb59ff2ea5..6e17d8ee38 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -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); diff --git a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index 65ea01c6e6..34d70c509d 100644 --- a/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -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) _param_mis_yaw_err, // yaw-error threshold (ParamFloat) _param_mpc_acc_hor, // acceleration in flight (ParamFloat) _param_mpc_acc_up_max, diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index c8c9f9f081..449a84be3c 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -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; +} diff --git a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp index 2a1cfbc299..bbd5d729e4 100644 --- a/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp +++ b/src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp @@ -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) _param_mpc_land_speed, (ParamFloat) _param_mpc_tiltmax_lnd, + (ParamInt) _param_mpc_land_rc_help, (ParamFloat) _param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed (ParamFloat) @@ -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. */ }; diff --git a/src/lib/flight_tasks/tasks/AutoMapper2/CMakeLists.txt b/src/lib/flight_tasks/tasks/AutoMapper2/CMakeLists.txt deleted file mode 100644 index 5c118c09fb..0000000000 --- a/src/lib/flight_tasks/tasks/AutoMapper2/CMakeLists.txt +++ /dev/null @@ -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}) diff --git a/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp deleted file mode 100644 index 5986036d41..0000000000 --- a/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp +++ /dev/null @@ -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 - -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; -} diff --git a/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp b/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp deleted file mode 100644 index 212270b961..0000000000 --- a/src/lib/flight_tasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp +++ /dev/null @@ -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) _param_mpc_land_speed, - (ParamFloat) _param_mpc_tiltmax_lnd, - (ParamInt) _param_mpc_land_rc_help, - (ParamFloat) - _param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed - (ParamFloat) - _param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed - (ParamFloat) _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. */ -}; diff --git a/src/lib/flight_tasks/tasks/CMakeLists.txt b/src/lib/flight_tasks/tasks/CMakeLists.txt index c4ecd8571e..306d9a326b 100644 --- a/src/lib/flight_tasks/tasks/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/CMakeLists.txt @@ -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() \ No newline at end of file +endforeach() diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 0cc394547a..4aed63d566 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -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 -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 */ diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp index bf8b8e8729..d11113f112 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp @@ -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); } } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a601e5828c..197a59a677 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -169,7 +169,6 @@ private: (ParamFloat) _param_mpc_xy_cruise, (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ (ParamInt) _param_mpc_pos_mode, - (ParamInt) _param_mpc_auto_mode, (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ (ParamFloat) _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) { diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 80e1ccccea..4f9e153d45 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -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 *