From d85481be6d631aeee5011ba27d82b4dc9c368d97 Mon Sep 17 00:00:00 2001 From: Martina Date: Thu, 19 Jul 2018 16:35:45 +0200 Subject: [PATCH] Restructure FlightTaskAutoLine: -add FlightTaskAutoMapper that handles the different types of waypoint and generates setpoints for all types except of position and loiter - FlightTaskAutoLine generates the setpoint types position and loiter if the flight between waypoint is a straight line --- src/lib/FlightTasks/CMakeLists.txt | 1 + .../FlightTasks/tasks/FlightTaskAutoLine.cpp | 124 +------------ .../FlightTasks/tasks/FlightTaskAutoLine.hpp | 39 +--- .../tasks/FlightTaskAutoMapper.cpp | 171 ++++++++++++++++++ .../tasks/FlightTaskAutoMapper.hpp | 93 ++++++++++ 5 files changed, 271 insertions(+), 157 deletions(-) create mode 100644 src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp create mode 100644 src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index 3cf7412c3c..d2adfaecb3 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -42,6 +42,7 @@ tasks/FlightTask.cpp tasks/FlightTaskManualPositionSmooth.cpp tasks/FlightTaskAuto.cpp tasks/FlightTaskAutoLine.cpp + tasks/FlightTaskAutoMapper.cpp tasks/FlightTaskAutoFollowMe.cpp tasks/FlightTaskOffboard.cpp tasks/Utility/ManualSmoothingZ.cpp diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index d0d19c80e8..f35b955931 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp @@ -45,108 +45,9 @@ using namespace matrix; bool FlightTaskAutoLine::activate() { - bool ret = FlightTaskAuto::activate(); - _reset(); - return ret; + return FlightTaskAutoMapper::activate(); } -bool FlightTaskAutoLine::update() -{ - // always reset constraints because they might change depending on the type - _setDefaultConstraints(); - - _updateAltitudeAboveGround(); - - 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. - - if (_type_previous == WaypointType::idle) { - _thrust_setpoint = Vector3f(NAN, NAN, NAN); - } - - if (_type == WaypointType::idle) { - _generateIdleSetpoints(); - - } else if (_type == WaypointType::land) { - _generateLandSetpoints(); - - } else if (follow_line) { - _generateSetpoints(); - - } else if (_type == WaypointType::takeoff) { - _generateTakeoffSetpoints(); - - } else if (_type == WaypointType::velocity) { - _generateVelocitySetpoints(); - } - - // update previous type - _type_previous = _type; - - return true; -} - -void FlightTaskAutoLine::_reset() -{ - // Set setpoints equal current state. - _velocity_setpoint = _velocity; - _position_setpoint = _position; - _yaw_setpoint = _yaw; - _destination = _position; - _origin = _position; - _speed_at_target = 0.0f; -} - -void FlightTaskAutoLine::_generateIdleSetpoints() -{ - // 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 FlightTaskAutoLine::_generateLandSetpoints() -{ - // Keep xy-position and go down with landspeed. */ - _position_setpoint = Vector3f(_target(0), _target(1), NAN); - _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, MPC_LAND_SPEED.get())); - // set constraints - _constraints.tilt = MPC_TILTMAX_LND.get(); - _constraints.speed_down = MPC_LAND_SPEED.get(); - _constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN; -} - -void FlightTaskAutoLine::_generateTakeoffSetpoints() -{ - // 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(_alt_above_ground, MPC_LAND_ALT2.get(), - MPC_LAND_ALT1.get(), MPC_TKO_SPEED.get(), _constraints.speed_up); - - _constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN; - -} - -void FlightTaskAutoLine::_generateVelocitySetpoints() -{ - // TODO: Remove velocity force logic from navigator, since - // navigator should only send out waypoints. - _position_setpoint = Vector3f(NAN, NAN, _position(2)); - Vector2f vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed; - _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN); -} void FlightTaskAutoLine::_generateSetpoints() { @@ -517,31 +418,8 @@ float FlightTaskAutoLine::_getVelocityFromAngle(const float angle) return math::constrain(speed_close, min_cruise_speed, _mc_cruise_speed); } -void FlightTaskAutoLine::_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; - } -} - bool FlightTaskAutoLine::_highEnoughForLandingGear() { // return true if altitude is above two meters return _alt_above_ground > 2.0f; } - -void FlightTaskAutoLine::updateParams() -{ - FlightTaskAuto::updateParams(); - - // make sure that alt1 is above alt2 - MPC_LAND_ALT1.set(math::max(MPC_LAND_ALT1.get(), MPC_LAND_ALT2.get())); -} diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp index 75f1a37b83..62b8256e14 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.hpp @@ -40,60 +40,31 @@ #pragma once -#include "FlightTaskAuto.hpp" +#include "FlightTaskAutoMapper.hpp" -class FlightTaskAutoLine : public FlightTaskAuto +class FlightTaskAutoLine : public FlightTaskAutoMapper { public: FlightTaskAutoLine() = default; virtual ~FlightTaskAutoLine() = default; bool activate() override; - bool update() override; protected: - matrix::Vector3f _destination{}; /**< Current target. Is not necessarily the same as triplet target. */ - matrix::Vector3f _origin{}; /**< Previous waypoint. Is not necessarily the same as triplet previous. */ - float _speed_at_target = 0.0f; /**< Desired velocity at target. */ - float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */ - - enum class State { - offtrack, /**< Vehicle is more than cruise speed away from track */ - target_behind, /**< Vehicle is in front of target. */ - previous_infront, /**< Vehilce is behind previous waypoint.*/ - none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */ - }; - State _current_state{State::none}; - - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, - (ParamFloat) MPC_LAND_SPEED, - (ParamFloat) MPC_CRUISE_90, // speed at corner when angle is 90 degrees - (ParamFloat) NAV_ACC_RAD, // acceptance radius at which waypoints are updated + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoLine, (ParamFloat) MIS_YAW_ERR, // yaw-error threshold - (ParamFloat) MPC_LAND_ALT1, // altitude at which speed limit downwards reaches maximum speed - (ParamFloat) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed (ParamFloat) MPC_ACC_UP_MAX, (ParamFloat) MPC_ACC_DOWN_MAX, - (ParamFloat) MPC_ACC_HOR, // acceleration in flight - (ParamFloat) MPC_TILTMAX_LND, - (ParamFloat) MPC_TKO_SPEED + (ParamFloat) MPC_ACC_HOR // acceleration in flight ) - void _generateIdleSetpoints(); - void _generateLandSetpoints(); - void _generateVelocitySetpoints(); - void _generateTakeoffSetpoints(); + void _generateSetpoints() override; void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */ - void _generateSetpoints(); /**< Generate velocity and position setpoint for following line. */ void _generateAltitudeSetpoints(); /**< Generate velocity and position setpoints for following line along z. */ - void _updateAltitudeAboveGround(); /**< Computes altitude above ground based on sensors available. */ void _generateXYsetpoints(); /**< Generate velocity and position setpoints for following line along xy. */ - void updateParams() override; /**< See ModuleParam class */ private: float _getVelocityFromAngle(const float angle); /**< Computes the speed at target depending on angle. */ bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */ - void _reset(); /**< Resets member variables to current vehicle state */ - WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */ }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp new file mode 100644 index 0000000000..0a45316ff0 --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.cpp @@ -0,0 +1,171 @@ +/**************************************************************************** + * + * 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 "FlightTaskAutoMapper.hpp" +#include + +using namespace matrix; + +#define SIGMA_SINGLE_OP 0.000001f +#define SIGMA_NORM 0.001f + +bool FlightTaskAutoMapper::activate() +{ + bool ret = FlightTaskAuto::activate(); + _reset(); + return ret; +} + +bool FlightTaskAutoMapper::update() +{ + // always reset constraints because they might change depending on the type + _setDefaultConstraints(); + + _updateAltitudeAboveGround(); + + 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. + + if (_type_previous == WaypointType::idle) { + _thrust_setpoint = Vector3f(NAN, NAN, NAN); + } + + if (_type == WaypointType::idle) { + _generateIdleSetpoints(); + + } else if (_type == WaypointType::land) { + _generateLandSetpoints(); + + } else if (follow_line) { + _generateSetpoints(); + + } else if (_type == WaypointType::takeoff) { + _generateTakeoffSetpoints(); + + } else if (_type == WaypointType::velocity) { + _generateVelocitySetpoints(); + } + + // update previous type + _type_previous = _type; + + return true; +} + +void FlightTaskAutoMapper::_reset() +{ + // Set setpoints equal current state. + _velocity_setpoint = _velocity; + _position_setpoint = _position; + _yaw_setpoint = _yaw; + _destination = _position; + _origin = _position; + _speed_at_target = 0.0f; +} + +void FlightTaskAutoMapper::_generateIdleSetpoints() +{ + // 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 FlightTaskAutoMapper::_generateLandSetpoints() +{ + // Keep xy-position and go down with landspeed. */ + _position_setpoint = Vector3f(_target(0), _target(1), NAN); + _velocity_setpoint = Vector3f(Vector3f(NAN, NAN, MPC_LAND_SPEED.get())); + // set constraints + _constraints.tilt = MPC_TILTMAX_LND.get(); + _constraints.speed_down = MPC_LAND_SPEED.get(); + _constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN; +} + +void FlightTaskAutoMapper::_generateTakeoffSetpoints() +{ + // 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(_alt_above_ground, MPC_LAND_ALT2.get(), + MPC_LAND_ALT1.get(), MPC_TKO_SPEED.get(), _constraints.speed_up); + + _constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN; +} + +void FlightTaskAutoMapper::_generateVelocitySetpoints() +{ + // TODO: Remove velocity force logic from navigator, since + // navigator should only send out waypoints. + _position_setpoint = Vector3f(NAN, NAN, _position(2)); + Vector2f vel_sp_xy = Vector2f(&_velocity(0)).unit_or_zero() * _mc_cruise_speed; + _velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN); +} + +void FlightTaskAutoMapper::_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 FlightTaskAutoMapper::updateParams() +{ + FlightTaskAuto::updateParams(); + + // make sure that alt1 is above alt2 + MPC_LAND_ALT1.set(math::max(MPC_LAND_ALT1.get(), MPC_LAND_ALT2.get())); +} diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp new file mode 100644 index 0000000000..d0a47036c8 --- /dev/null +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoMapper.hpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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 FlightTaskAutoMapper.hpp + * + * Flight task for autonomous, gps driven mode. The vehicle flies + * along a straight line in between waypoints. + */ + +#pragma once + +#include "FlightTaskAuto.hpp" + + +class FlightTaskAutoMapper : public FlightTaskAuto +{ +public: + FlightTaskAutoMapper() = default; + virtual ~FlightTaskAutoMapper() = default; + bool activate() override; + bool update() override; + +protected: + + matrix::Vector3f _destination{}; /**< Current target. Is not necessarily the same as triplet target. */ + matrix::Vector3f _origin{}; /**< Previous waypoint. Is not necessarily the same as triplet previous. */ + float _speed_at_target = 0.0f; /**< Desired velocity at target. */ + float _alt_above_ground{0.0f}; /**< If home provided, then it is altitude above home, otherwise it is altitude above local position reference. */ + + enum class State { + offtrack, /**< Vehicle is more than cruise speed away from track */ + target_behind, /**< Vehicle is in front of target. */ + previous_infront, /**< Vehilce is behind previous waypoint.*/ + none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */ + }; + State _current_state{State::none}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper, + (ParamFloat) MPC_LAND_SPEED, + (ParamFloat) MPC_CRUISE_90, // speed at corner when angle is 90 degrees move to line + (ParamFloat) NAV_ACC_RAD, // acceptance radius at which waypoints are updated move to line + (ParamFloat) MPC_LAND_ALT1, // altitude at which speed limit downwards reaches maximum speed + (ParamFloat) MPC_LAND_ALT2, // altitude at which speed limit downwards reached minimum speed + (ParamFloat) MPC_TILTMAX_LND, + (ParamFloat) MPC_TKO_SPEED + ) + + virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */ + + void _generateIdleSetpoints(); + void _generateLandSetpoints(); + void _generateVelocitySetpoints(); + void _generateTakeoffSetpoints(); + + 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. */ + +};