diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index 95b03ea486e8..cb706fa96bd0 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/FlightTaskAutoSmooth.cpp tasks/FlightTaskOffboard.cpp diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoLine.cpp index d0d19c80e8c5..f35b95593186 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 75f1a37b83e1..62b8256e149c 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 000000000000..0a45316ff006 --- /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 000000000000..d0a47036c8ab --- /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. */ + +}; diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoSmooth.cpp b/src/lib/FlightTasks/tasks/FlightTaskAutoSmooth.cpp index 9288eae0e27d..96a0e7e38d84 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoSmooth.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoSmooth.cpp @@ -43,12 +43,12 @@ using namespace matrix; #define SIGMA_SINGLE_OP 0.000001f FlightTaskAutoSmooth::FlightTaskAutoSmooth() : - _sl(nullptr, _deltatime, _position, _velocity) + _sl(nullptr, _deltatime, _position) { } bool FlightTaskAutoSmooth::activate() { - bool ret = FlightTaskAuto::activate(); + bool ret = FlightTaskAutoMapper::activate(); ret = ret && PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1)) @@ -56,12 +56,12 @@ bool FlightTaskAutoSmooth::activate() && PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1)) && PX4_ISFINITE(_velocity(2)); - return ret; } -bool FlightTaskAutoSmooth::update() +void FlightTaskAutoSmooth::_generateSetpoints() { + if (_control_points_update) { _update_control_points(); _control_points_update = false; @@ -98,6 +98,7 @@ bool FlightTaskAutoSmooth::update() if (_pt_0_reached_once && !pt_1_reached) { _b.getStatesClosest(_position_setpoint, _velocity_setpoint, acceleration, _position); + } else if (!pt_1_reached) { _sl.generateSetpoints(_position_setpoint, _velocity_setpoint); } @@ -106,9 +107,6 @@ bool FlightTaskAutoSmooth::update() _control_points_update = true; _pt_0_reached_once = false; } - - return true; - } void FlightTaskAutoSmooth::_update_control_points() diff --git a/src/lib/FlightTasks/tasks/FlightTaskAutoSmooth.hpp b/src/lib/FlightTasks/tasks/FlightTaskAutoSmooth.hpp index 2935472713ea..d024999cfffb 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskAutoSmooth.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskAutoSmooth.hpp @@ -40,25 +40,25 @@ #pragma once -#include "FlightTaskAuto.hpp" +#include "FlightTaskAutoMapper.hpp" #include "lib/bezier/BezierQuad.hpp" #include "Utility/StraightLine.hpp" -class FlightTaskAutoSmooth : public FlightTaskAuto +class FlightTaskAutoSmooth : public FlightTaskAutoMapper { public: FlightTaskAutoSmooth(); virtual ~FlightTaskAutoSmooth() = default; bool activate() override; - bool update() override; protected: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto, (ParamFloat) NAV_ACC_RAD, // acceptance radius at which waypoints are updated (ParamFloat) MPC_CRUISE_90 // speed at corner when angle is 90 degrees - ) + ) void updateParams() override; /**< See ModuleParam class */ + void _generateSetpoints() override; private: bezier::BezierQuadf _b; diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp index 7f7000030d9b..b3c8f849a9f9 100644 --- a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp +++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp @@ -39,28 +39,23 @@ #include #include -#define ACC_RAD_ZERO_VEL 2.0f -#define VEL_ZERO_THRESHOLD 0.001f -#define DECELERATION_MAX 8.0f +#define VEL_ZERO_THRESHOLD 0.001f // Threshold to compare if the target velocity is zero +#define DECELERATION_MAX 8.0f // The vehicles maximum deceleration TODO check to create param using namespace matrix; -StraightLine::StraightLine(ModuleParams *parent, const float &deltatime, const matrix::Vector3f &pos, - const matrix::Vector3f &vel) : +StraightLine::StraightLine(ModuleParams *parent, const float &deltatime, const matrix::Vector3f &pos) : ModuleParams(parent), - _deltatime(deltatime), _pos(pos), _vel(vel) + _deltatime(deltatime), _pos(pos) { - // printf("StraightLine::StraightLine \n"); + } void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint) { - // printf("_is_target_reached %d %d %d \n", _is_target_reached, _desired_speed_at_target < VEL_ZERO_THRESHOLD, (_pos - _target).length() < ACC_RAD_ZERO_VEL); // Check if target position has been reached - // printf("pos %f %f %f \n", (double)_pos(0), (double)_pos(1), (double)_pos(2)); - // printf("_target %f %f %f \n", (double)_target(0), (double)_target(1), (double)_target(2)); if (_is_target_reached || (_desired_speed_at_target < VEL_ZERO_THRESHOLD && - (_pos - _target).length() < ACC_RAD_ZERO_VEL)) { + (_pos - _target).length() < NAV_ACC_RAD.get())) { // Vehicle has reached target. Lock position position_setpoint = _target; velocity_setpoint = Vector3f(0.0f, 0.0f, 0.0f); @@ -78,17 +73,15 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix // previous velocity in the direction of the line float speed_sp_prev = math::max(velocity_setpoint * u_orig_to_target, 0.0f); - // Calculate braking distance depending on speed, speed at target and deceleration (add 10% safety margin) - float braking_distance = 1.1f * ((powf(_desired_speed, 2) - powf(_desired_speed_at_target, 2)) / (2.0f * _desired_deceleration)); + // Calculate accelerating/decelerating distance depending on speed, speed at target and acceleration/deceleration (add 10% safety margin) + float acc_dec_distance = 1.1f * fabs(powf(_desired_speed, 2) - powf(_desired_speed_at_target, 2)) / 2.0f; + acc_dec_distance /= _desired_speed > _desired_speed_at_target ? _desired_deceleration : _desired_acceleration; float dist_to_target = (_target - _pos).length(); // distance to target - // printf("_desired_speed = %f _desired_speed_at_target = %f\n", (double)_desired_speed, (double)_desired_speed_at_target); - // printf("braking_distance = %f dist_to_target = %f\n", (double)braking_distance, (double)dist_to_target); // Either accelerate or decelerate - float speed_sp = dist_to_target > braking_distance ? _desired_speed : _desired_speed_at_target; - // printf("speed_sp1 = %f\n", (double)speed_sp); - float max_acc_dec = dist_to_target > braking_distance ? _desired_acceleration : -_desired_deceleration; + float speed_sp = dist_to_target > acc_dec_distance ? _desired_speed : _desired_speed_at_target; + float max_acc_dec = speed_sp > speed_sp_prev ? _desired_acceleration : -_desired_deceleration; float acc_track = (speed_sp - speed_sp_prev) / _deltatime; @@ -96,13 +89,10 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix // accelerate/decelerate with desired acceleration/deceleration towards target speed_sp = speed_sp_prev + max_acc_dec * _deltatime; } - // printf("speed_sp2 = %f\n", (double)speed_sp); // constrain the velocity speed_sp = math::constrain(speed_sp, 0.0f, _desired_speed); - // printf("_pos = %f %f %f _target = %f %f %f\n", (double)_pos(0), (double)_pos(1), (double)_pos(2), (double)_target(0), (double)_target(1), (double)_target(2)); - // printf("speed_sp3 = %f\n", (double)speed_sp); - // printf("position_setpoint = %f %f %f velocity_setpoint = %f %f %f\n", (double)position_setpoint(0), (double)position_setpoint(1), (double)position_setpoint(2), (double)velocity_setpoint(0), (double)velocity_setpoint(1), (double)velocity_setpoint(2)); + // set the position and velocity setpoints position_setpoint = closest_pt_on_line; velocity_setpoint = u_orig_to_target * speed_sp; @@ -120,6 +110,7 @@ float StraightLine::getMaxAcc() if (divider > FLT_EPSILON) { max_acc_hor /= divider; + } else { max_acc_hor *= 1000.0f; } @@ -130,6 +121,7 @@ float StraightLine::getMaxAcc() if (fabs(u_orig_to_target(2)) > FLT_EPSILON) { max_acc_vert /= fabs(u_orig_to_target(2)); + } else { max_acc_vert *= 1000.0f; } @@ -148,6 +140,7 @@ float StraightLine::getMaxVel() if (divider > FLT_EPSILON) { max_vel_hor /= divider; + } else { max_vel_hor *= 1000.0f; } @@ -158,6 +151,7 @@ float StraightLine::getMaxVel() if (fabs(u_orig_to_target(2)) > FLT_EPSILON) { max_vel_vert /= fabs(u_orig_to_target(2)); + } else { max_vel_vert *= 1000.0f; } @@ -175,11 +169,10 @@ void StraightLine::setAllDefaults() void StraightLine::setTarget(const matrix::Vector3f &target) { - // printf("StraightLine::setTarget %f %f %f \n", (double)target(0), (double)target(1), (double)target(2)); if (PX4_ISFINITE(target(0)) && PX4_ISFINITE(target(1)) && PX4_ISFINITE(target(2))) { _target = target; _is_target_reached = false; - // printf("StraightLine::setTarget \n"); + // set all parameters to their default value (depends on the direction) setAllDefaults(); } @@ -187,7 +180,6 @@ void StraightLine::setTarget(const matrix::Vector3f &target) void StraightLine::setOrigin(const matrix::Vector3f &origin) { - // printf("StraightLine::setOrigin %f %f %f \n", (double)origin(0), (double)origin(1), (double)origin(2)); if (PX4_ISFINITE(origin(0)) && PX4_ISFINITE(origin(1)) && PX4_ISFINITE(origin(2))) { _origin = origin; } @@ -202,7 +194,6 @@ void StraightLine::setSpeed(const float &speed) void StraightLine::setSpeedAtTarget(const float &speed_at_target) { - // printf("setSpeedAtTarget speed_at_target %f < getMaxVel() %f \n", (double)speed_at_target, (double)getMaxVel() ); if (speed_at_target > 0 && speed_at_target < getMaxVel()) { _desired_speed_at_target = speed_at_target; } diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp index e2571f789752..131fb96d96e0 100644 --- a/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp +++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp @@ -47,7 +47,7 @@ class StraightLine : public ModuleParams { public: - StraightLine(ModuleParams *parent, const float &deltatime, const matrix::Vector3f &pos, const matrix::Vector3f &vel); + StraightLine(ModuleParams *parent, const float &deltatime, const matrix::Vector3f &pos); ~StraightLine() = default; // setter functions @@ -86,7 +86,6 @@ class StraightLine : public ModuleParams private: const float &_deltatime; /**< delta time between last update (dependency injection) */ const matrix::Vector3f &_pos; /**< vehicle position (dependency injection) */ - const matrix::Vector3f &_vel; /**< vehicle velocity (dependency injection) */ float _desired_acceleration{0.0f}; /**< acceleration along the straight line */ float _desired_deceleration{0.0f}; /**< deceleration along the straight line */ @@ -105,7 +104,8 @@ class StraightLine : public ModuleParams (ParamFloat) MPC_ACC_DOWN_MAX, /**< maximum vertical acceleration downwards*/ (ParamFloat) MPC_XY_VEL_MAX, /**< maximum horizontal velocity */ (ParamFloat) MPC_Z_VEL_MAX_UP, /**< maximum vertical velocity upwards */ - (ParamFloat) MPC_Z_VEL_MAX_DN /**< maximum vertical velocity downwards */ + (ParamFloat) MPC_Z_VEL_MAX_DN, /**< maximum vertical velocity downwards */ + (ParamFloat) NAV_ACC_RAD /**< acceptance radius if a waypoint is reached */ ) };