From 3c5b24037a2a4c88d86cc59f1c8c4897a7003f9c Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 4 Sep 2019 14:16:03 +0200 Subject: [PATCH] AutoLineSmoothVel - Improve virtual pilot logic that provides velocity setpoints to the trajectory generator. Constrain the generated velocities in the NE inertial frame rather than just constraining the norm. --- .../FlightTaskAutoLineSmoothVel.cpp | 52 ++++++++++++++----- .../FlightTaskAutoLineSmoothVel.hpp | 5 +- 2 files changed, 43 insertions(+), 14 deletions(-) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index 835e00c99e..b6b03fad6a 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -135,18 +135,29 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj() return res; } -/* Constrain some value vith a constrain depending on the sign of the constrain +/* Constrain some value vith a constrain depending on the sign of the constraint * Example: - if the constrain is -5, the value will be constrained between -5 and 0 * - if the constrain is 5, the value will be constrained between 0 and 5 */ -inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constrain) +inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint) { - const float min = (constrain < FLT_EPSILON) ? constrain : 0.f; - const float max = (constrain > FLT_EPSILON) ? constrain : 0.f; + const float min = (constraint < FLT_EPSILON) ? constraint : 0.f; + const float max = (constraint > FLT_EPSILON) ? constraint : 0.f; return math::constrain(val, min, max); } +float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float constraint) +{ + float constraint_abs = fabsf(constraint); + return math::constrain(val, -constraint_abs, constraint_abs); +} + +float FlightTaskAutoLineSmoothVel::_maxAbs(float val, float constraint) +{ + return math::sign(val) * math::max(fabsf(val), fabsf(constraint)); +} + void FlightTaskAutoLineSmoothVel::_initEkfResetCounters() { _reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter; @@ -246,34 +257,49 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() PX4_ISFINITE(_position_setpoint(1))) { // Use position setpoints to generate velocity setpoints - // Get various path specific vectors. */ + // Get various path specific vectors Vector3f pos_traj; pos_traj(0) = _trajectory[0].getCurrentPosition(); pos_traj(1) = _trajectory[1].getCurrentPosition(); pos_traj(2) = _trajectory[2].getCurrentPosition(); Vector2f pos_traj_to_dest_xy(_position_setpoint - pos_traj); Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero()); - const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get(); - float speed_sp_track = _getMaxSpeedFromDistance(pos_traj_to_dest_xy.length()); + // Unconstrained desired velocity vector + Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * _mc_cruise_speed; + + Vector2f vel_max_xy; + vel_max_xy(0) = _getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(0))); + vel_max_xy(1) = _getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(1))); + + const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get(); + Vector2f vel_min_xy; if (has_reached_altitude) { - speed_sp_track = math::constrain(speed_sp_track, _getSpeedAtTarget(), _mc_cruise_speed); + // Compute the minimum speed in NE frame. This is used + // to force the drone to pass the waypoint with a desired speed + Vector2f u_prev_to_target_xy((_target - _prev_wp).unit_or_zero()); + vel_min_xy = u_prev_to_target_xy * _getSpeedAtTarget(); + vel_min_xy(0) = fabsf(vel_min_xy(0)); + vel_min_xy(1) = fabsf(vel_min_xy(1)); } else { - speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed); - + // The drone has to change altitude, stop at the waypoint + vel_min_xy.setAll(0.f); } - Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track; + // Constrain the norm of each component using min and max values + Vector2f vel_sp_constrained_xy; + vel_sp_constrained_xy(0) = _maxAbs(_constrainAbs(vel_sp_xy(0), vel_max_xy(0)), vel_min_xy(0)); + vel_sp_constrained_xy(1) = _maxAbs(_constrainAbs(vel_sp_xy(1), vel_max_xy(1)), vel_min_xy(1)); for (int i = 0; i < 2; i++) { // If available, constrain the velocity using _velocity_setpoint(.) if (PX4_ISFINITE(_velocity_setpoint(i))) { - _velocity_setpoint(i) = _constrainOneSide(vel_sp_xy(i), _velocity_setpoint(i)); + _velocity_setpoint(i) = _constrainOneSide(vel_sp_constrained_xy(i), _velocity_setpoint(i)); } else { - _velocity_setpoint(i) = vel_sp_xy(i); + _velocity_setpoint(i) = vel_sp_constrained_xy(i); } } diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index df701d1223..4f344812d6 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -72,7 +72,10 @@ protected: /** determines when to trigger a takeoff (ignored in flight) */ bool _checkTakeoff() override { return _want_takeoff; }; - inline float _constrainOneSide(float val, float constrain); + inline float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */ + inline float _constrainAbs(float val, float constraint); /**< Constrain the absolute value of val */ + inline float _maxAbs(float val, float constraint); /**< Maximum of val and constraint with the sign of val */ + void _initEkfResetCounters(); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ void _generateHeading();