From f5d7eb4d876011f0056438e5ca01b96987241529 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 11 Sep 2019 18:22:23 +0200 Subject: [PATCH] Refactor FlightTaskManualPositionSmoothVel --- .../FlightTaskManualPositionSmoothVel.cpp | 212 ++++-------------- .../FlightTaskManualPositionSmoothVel.hpp | 50 +---- .../FlightTasks/tasks/Utility/CMakeLists.txt | 2 + .../Utility/ManualVelocitySmoothingXY.cpp | 121 ++++++++++ .../Utility/ManualVelocitySmoothingXY.hpp | 110 +++++++++ .../Utility/ManualVelocitySmoothingZ.cpp | 118 ++++++++++ .../Utility/ManualVelocitySmoothingZ.hpp | 112 +++++++++ 7 files changed, 510 insertions(+), 215 deletions(-) create mode 100644 src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp create mode 100644 src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp create mode 100644 src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp create mode 100644 src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index cde73591bb7d..1315b39f9fa8 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019 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 @@ -44,17 +44,13 @@ bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint // Check if the previous FlightTask provided setpoints checkSetpoints(last_setpoint); - const Vector3f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y, last_setpoint.acc_z); - const Vector3f vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz); - const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z); + const Vector2f accel_prev(last_setpoint.acc_x, last_setpoint.acc_y); + const Vector2f vel_prev(last_setpoint.vx, last_setpoint.vy); + const Vector2f pos_prev(last_setpoint.x, last_setpoint.y); - for (int i = 0; i < 2; ++i) { - _smoothing_xy[i].reset(accel_prev(i), vel_prev(i), pos_prev(i)); - } - - _smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2)); + _smoothing_xy.reset(accel_prev, vel_prev, pos_prev); + _smoothing_z.reset(last_setpoint.acc_z, last_setpoint.vz, last_setpoint.z); - _resetPositionLock(); _initEkfResetCounters(); return ret; @@ -64,13 +60,9 @@ void FlightTaskManualPositionSmoothVel::reActivate() { // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly // using the generated jerk, reset the z derivatives to zero - for (int i = 0; i < 2; ++i) { - _smoothing_xy[i].reset(0.f, _velocity(i), _position(i)); - } - + _smoothing_xy.reset(Vector2f(), Vector2f(_velocity), Vector2f(_position)); _smoothing_z.reset(0.f, 0.f, _position(2)); - _resetPositionLock(); _initEkfResetCounters(); } @@ -98,25 +90,6 @@ void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_se if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; } } -void FlightTaskManualPositionSmoothVel::_resetPositionLock() -{ - _resetPositionLockXY(); - _resetPositionLockZ(); -} - -void FlightTaskManualPositionSmoothVel::_resetPositionLockXY() -{ - _position_lock_xy_active = false; - _position_setpoint_xy_locked(0) = NAN; - _position_setpoint_xy_locked(1) = NAN; -} - -void FlightTaskManualPositionSmoothVel::_resetPositionLockZ() -{ - _position_lock_z_active = false; - _position_setpoint_z_locked = NAN; -} - void FlightTaskManualPositionSmoothVel::_initEkfResetCounters() { _initEkfResetCountersXY(); @@ -140,25 +113,16 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() // Reset trajectories if EKF did a reset _checkEkfResetCounters(); - // Update state - _updateTrajectories(); - // Set max accel/vel/jerk - // Has to be done before _updateTrajDurations() + // Has to be done before _updateTrajectories() _updateTrajConstraints(); + _updateTrajVelFeedback(); + _updateTrajCurrentPositionEstimate(); - // Get yaw setpont, un-smoothed position setpoints - // Has to be done before _checkPositionLock() + // Get yaw setpoint, un-smoothed position setpoints FlightTaskManualPosition::_updateSetpoints(); - _velocity_target_xy = Vector2f(_velocity_setpoint); - _velocity_target_z = _velocity_setpoint(2); - // Lock or unlock position - // Has to be done before _updateTrajDurations() - _checkPositionLock(); - - // Update durations and sync XY - _updateTrajDurations(); + _updateTrajectories(_velocity_setpoint); // Fill the jerk, acceleration, velocity and position setpoint vectors _setOutputState(); @@ -174,14 +138,12 @@ void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters() void FlightTaskManualPositionSmoothVel::_checkEkfResetCountersXY() { if (_sub_vehicle_local_position->get().xy_reset_counter != _reset_counters.xy) { - _smoothing_xy[0].setCurrentPosition(_position(0)); - _smoothing_xy[1].setCurrentPosition(_position(1)); + _smoothing_xy.setCurrentPosition(Vector2f(_position)); _reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter; } if (_sub_vehicle_local_position->get().vxy_reset_counter != _reset_counters.vxy) { - _smoothing_xy[0].setCurrentVelocity(_velocity(0)); - _smoothing_xy[1].setCurrentVelocity(_velocity(1)); + _smoothing_xy.setCurrentVelocity(Vector2f(_velocity)); _reset_counters.vxy = _sub_vehicle_local_position->get().vxy_reset_counter; } } @@ -199,34 +161,6 @@ void FlightTaskManualPositionSmoothVel::_checkEkfResetCountersZ() } } -void FlightTaskManualPositionSmoothVel::_updateTrajectories() -{ - _updateTrajectoriesXY(); - _updateTrajectoriesZ(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajectoriesXY() -{ - for (int i = 0; i < 2; ++i) { - _smoothing_xy[i].updateTraj(_deltatime); - - _traj_xy.j(i) = _smoothing_xy[i].getCurrentJerk(); - _traj_xy.a(i) = _smoothing_xy[i].getCurrentAcceleration(); - _traj_xy.v(i) = _smoothing_xy[i].getCurrentVelocity(); - _traj_xy.x(i) = _smoothing_xy[i].getCurrentPosition(); - } -} - -void FlightTaskManualPositionSmoothVel::_updateTrajectoriesZ() -{ - _smoothing_z.updateTraj(_deltatime); - - _traj_z.j = _smoothing_z.getCurrentJerk(); - _traj_z.a = _smoothing_z.getCurrentAcceleration(); - _traj_z.v = _smoothing_z.getCurrentVelocity(); - _traj_z.x = _smoothing_z.getCurrentPosition(); -} - void FlightTaskManualPositionSmoothVel::_updateTrajConstraints() { _updateTrajConstraintsXY(); @@ -235,106 +169,38 @@ void FlightTaskManualPositionSmoothVel::_updateTrajConstraints() void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY() { - for (int i = 0; i < 2; i++) { - _smoothing_xy[i].setMaxJerk(_param_mpc_jerk_max.get()); - _smoothing_xy[i].setMaxAccel(_param_mpc_acc_hor_max.get()); - _smoothing_xy[i].setMaxVel(_constraints.speed_xy); - } + _smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get()); + _smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get()); + _smoothing_xy.setMaxVel(_constraints.speed_xy); } void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ() { _smoothing_z.setMaxJerk(_param_mpc_jerk_max.get()); - if (_velocity_setpoint(2) < 0.f) { // up - _smoothing_z.setMaxAccel(_param_mpc_acc_up_max.get()); - _smoothing_z.setMaxVel(_constraints.speed_up); - - } else { // down - _smoothing_z.setMaxAccel(_param_mpc_acc_down_max.get()); - _smoothing_z.setMaxVel(_constraints.speed_down); - } -} - -void FlightTaskManualPositionSmoothVel::_updateTrajDurations() -{ - _updateTrajDurationsXY(); - _updateTrajDurationsZ(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajDurationsXY() -{ - for (int i = 0; i < 2; ++i) { - _smoothing_xy[i].updateDurations(_velocity_target_xy(i)); - } + _smoothing_z.setMaxAccelUp(_param_mpc_acc_up_max.get()); + _smoothing_z.setMaxVelUp(_constraints.speed_up); - VelocitySmoothing::timeSynchronization(_smoothing_xy, 2); + _smoothing_z.setMaxAccelDown(_param_mpc_acc_down_max.get()); + _smoothing_z.setMaxVelDown(_constraints.speed_down); } -void FlightTaskManualPositionSmoothVel::_updateTrajDurationsZ() +void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback() { - _smoothing_z.updateDurations(_velocity_target_z); + _smoothing_xy.setVelSpFeedback(Vector2f(_velocity_setpoint_feedback)); + _smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2)); } -void FlightTaskManualPositionSmoothVel::_checkPositionLock() +void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate() { - /** - * During a position lock -> position unlock transition, we have to make sure that the velocity setpoint - * is continuous. We know that the output of the position loop (part of the velocity setpoint) - * will suddenly become null - * and only the feedforward (generated by this flight task) will remain. - * This is why the previous input of the velocity controller - * is used to set current velocity of the trajectory. - */ - _checkPositionLockXY(); - _checkPositionLockZ(); + _smoothing_xy.setCurrentPositionEstimate(Vector2f(_position)); + _smoothing_z.setCurrentPositionEstimate(_position(2)); } -void FlightTaskManualPositionSmoothVel::_checkPositionLockXY() +void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target) { - if (_traj_xy.v.length() < 0.1f && - _traj_xy.a.length() < .2f && - _velocity_target_xy.length() <= FLT_EPSILON) { - // Lock position - _position_lock_xy_active = true; - _position_setpoint_xy_locked = _traj_xy.x; - - } else { - // Unlock position - if (_position_lock_xy_active) { - _smoothing_xy[0].setCurrentVelocity(_velocity_setpoint_feedback( - 0)); // Start the trajectory at the current velocity setpoint - _smoothing_xy[1].setCurrentVelocity(_velocity_setpoint_feedback(1)); - _position_setpoint_xy_locked(0) = NAN; - _position_setpoint_xy_locked(1) = NAN; - } - - _position_lock_xy_active = false; - _smoothing_xy[0].setCurrentPosition(_position(0)); - _smoothing_xy[1].setCurrentPosition(_position(1)); - } -} - -void FlightTaskManualPositionSmoothVel::_checkPositionLockZ() -{ - if (fabsf(_traj_z.v) < 0.1f && - fabsf(_traj_z.a) < .2f && - fabsf(_velocity_target_z) <= FLT_EPSILON) { - // Lock position - _position_lock_z_active = true; - _position_setpoint_z_locked = _traj_z.x; - - } else { - // Unlock position - if (_position_lock_z_active) { - _smoothing_z.setCurrentVelocity(_velocity_setpoint_feedback( - 2)); // Start the trajectory at the current velocity setpoint - _position_setpoint_z_locked = NAN; - } - - _position_lock_z_active = false; - _smoothing_z.setCurrentPosition(_position(2)); - } + _smoothing_xy.update(_deltatime, Vector2f(vel_target)); + _smoothing_z.update(_deltatime, vel_target(2)); } void FlightTaskManualPositionSmoothVel::_setOutputState() @@ -345,18 +211,16 @@ void FlightTaskManualPositionSmoothVel::_setOutputState() void FlightTaskManualPositionSmoothVel::_setOutputStateXY() { - for (int i = 0; i < 2; i++) { - _jerk_setpoint(i) = _traj_xy.j(i); - _acceleration_setpoint(i) = _traj_xy.a(i); - _velocity_setpoint(i) = _traj_xy.v(i); - _position_setpoint(i) = _position_setpoint_xy_locked(i); - } + _jerk_setpoint = _smoothing_xy.getCurrentJerk(); + _acceleration_setpoint = _smoothing_xy.getCurrentAcceleration(); + _velocity_setpoint = _smoothing_xy.getCurrentVelocity(); + _position_setpoint = _smoothing_xy.getCurrentPosition(); } void FlightTaskManualPositionSmoothVel::_setOutputStateZ() { - _jerk_setpoint(2) = _traj_z.j; - _acceleration_setpoint(2) = _traj_z.a; - _velocity_setpoint(2) = _traj_z.v; - _position_setpoint(2) = _position_setpoint_z_locked; + _jerk_setpoint(2) = _smoothing_z.getCurrentJerk(); + _acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration(); + _velocity_setpoint(2) = _smoothing_z.getCurrentVelocity(); + _position_setpoint(2) = _smoothing_z.getCurrentPosition(); } diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index cc5814fc9eca..b6399645c5e8 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019 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 @@ -40,9 +40,11 @@ #pragma once #include "FlightTaskManualPosition.hpp" -#include "VelocitySmoothing.hpp" +#include "ManualVelocitySmoothingXY.hpp" +#include "ManualVelocitySmoothingZ.hpp" using matrix::Vector2f; +using matrix::Vector3f; class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition { @@ -67,10 +69,6 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition private: void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); - void _resetPositionLock(); - void _resetPositionLockXY(); - void _resetPositionLockZ(); - void _initEkfResetCounters(); void _initEkfResetCountersXY(); void _initEkfResetCountersZ(); @@ -79,37 +77,21 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition void _checkEkfResetCountersXY(); void _checkEkfResetCountersZ(); - void _updateTrajectories(); - void _updateTrajectoriesXY(); - void _updateTrajectoriesZ(); - void _updateTrajConstraints(); void _updateTrajConstraintsXY(); void _updateTrajConstraintsZ(); - void _updateTrajDurations(); - void _updateTrajDurationsXY(); - void _updateTrajDurationsZ(); + void _updateTrajVelFeedback(); + void _updateTrajCurrentPositionEstimate(); - void _checkPositionLock(); - void _checkPositionLockXY(); - void _checkPositionLockZ(); + void _updateTrajectories(Vector3f vel_target); void _setOutputState(); void _setOutputStateXY(); void _setOutputStateZ(); - VelocitySmoothing _smoothing_xy[2]; ///< Smoothing in x and y directions - VelocitySmoothing _smoothing_z; ///< Smoothing in z direction - - Vector2f _velocity_target_xy; - float _velocity_target_z{0.f}; - - bool _position_lock_xy_active{false}; - bool _position_lock_z_active{false}; - - Vector2f _position_setpoint_xy_locked; - float _position_setpoint_z_locked{NAN}; + ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions + ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction /* counters for estimator local position resets */ struct { @@ -118,18 +100,4 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition uint8_t z; uint8_t vz; } _reset_counters{0, 0, 0, 0}; - - struct { - Vector2f j; - Vector2f a; - Vector2f v; - Vector2f x; - } _traj_xy; - - struct { - float j; - float a; - float v; - float x; - } _traj_z{0.f, 0.f, 0.f, NAN}; }; diff --git a/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt index 14cf505b344e..5cbaf2cfbe42 100644 --- a/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt +++ b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt @@ -37,6 +37,8 @@ px4_add_library(FlightTaskUtility ObstacleAvoidance.cpp StraightLine.cpp VelocitySmoothing.cpp + ManualVelocitySmoothingXY.cpp + ManualVelocitySmoothingZ.cpp ) target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis) diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp new file mode 100644 index 000000000000..ef1a0554d24c --- /dev/null +++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "ManualVelocitySmoothingXY.hpp" + +#include +#include + +using namespace matrix; + +void ManualVelocitySmoothingXY::reset(Vector2f accel, Vector2f vel, Vector2f pos) +{ + for (int i = 0; i < 2; i++) { + _smoothing[i].reset(accel(i), vel(i), pos(i)); + } + + resetPositionLock(); +} +void ManualVelocitySmoothingXY::resetPositionLock() +{ + _position_lock_active = false; + _position_setpoint_locked(0) = NAN; + _position_setpoint_locked(1) = NAN; +} + +void ManualVelocitySmoothingXY::update(float dt, Vector2f velocity_target) +{ + // Update state + updateTrajectories(dt); + + // Lock or unlock position + // Has to be done before _updateTrajDurations() + checkPositionLock(velocity_target); + + // Update durations and sync XY + updateTrajDurations(velocity_target); +} + +void ManualVelocitySmoothingXY::updateTrajectories(float dt) +{ + for (int i = 0; i < 2; ++i) { + _smoothing[i].updateTraj(dt); + + _state.j(i) = _smoothing[i].getCurrentJerk(); + _state.a(i) = _smoothing[i].getCurrentAcceleration(); + _state.v(i) = _smoothing[i].getCurrentVelocity(); + _state.x(i) = _smoothing[i].getCurrentPosition(); + } +} + +void ManualVelocitySmoothingXY::updateTrajDurations(Vector2f velocity_target) +{ + for (int i = 0; i < 2; ++i) { + _smoothing[i].updateDurations(velocity_target(i)); + } + + VelocitySmoothing::timeSynchronization(_smoothing, 2); +} + +void ManualVelocitySmoothingXY::checkPositionLock(Vector2f velocity_target) +{ + /** + * During a position lock -> position unlock transition, we have to make sure that the velocity setpoint + * is continuous. We know that the output of the position loop (part of the velocity setpoint) + * will suddenly become null + * and only the feedforward (generated by this flight task) will remain. + * This is why the previous input of the velocity controller + * is used to set current velocity of the trajectory. + */ + if (_state.v.length() < 0.1f && + _state.a.length() < .2f && + velocity_target.length() <= FLT_EPSILON) { + // Lock position + _position_lock_active = true; + _position_setpoint_locked = _state.x; + + } else { + // Unlock position + if (_position_lock_active) { + // Start the trajectory at the current velocity setpoint + _smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(0)); + _smoothing[1].setCurrentVelocity(_velocity_setpoint_feedback(1)); + _position_setpoint_locked(0) = NAN; + _position_setpoint_locked(1) = NAN; + } + + _position_lock_active = false; + _smoothing[0].setCurrentPosition(_position_estimate(0)); + _smoothing[1].setCurrentPosition(_position_estimate(1)); + } +} diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp new file mode 100644 index 000000000000..363d514cd62d --- /dev/null +++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 ManualVelocitySmoothingXY.hpp + * + */ + +#pragma once + +#include "VelocitySmoothing.hpp" + +#include + +using matrix::Vector2f; + +class ManualVelocitySmoothingXY +{ +public: + ManualVelocitySmoothingXY() = default; + + virtual ~ManualVelocitySmoothingXY() = default; + + void reset(Vector2f accel, Vector2f vel, Vector2f pos); + void update(float dt, Vector2f velocity_target); + + void setVelSpFeedback(const Vector2f fb) { _velocity_setpoint_feedback = fb; } + + void setMaxJerk(const float max_jerk) { + _smoothing[0].setMaxJerk(max_jerk); + _smoothing[1].setMaxJerk(max_jerk); + } + void setMaxAccel(const float max_accel) { + _smoothing[0].setMaxAccel(max_accel); + _smoothing[1].setMaxAccel(max_accel); + } + void setMaxVel(const float max_vel) { + _smoothing[0].setMaxVel(max_vel); + _smoothing[1].setMaxVel(max_vel); + } + + Vector2f getCurrentJerk() const { return _state.j; } + Vector2f getCurrentAcceleration() const { return _state.a; } + void setCurrentVelocity(const Vector2f vel) { + _state.v = vel; + _smoothing[0].setCurrentVelocity(vel(0)); + _smoothing[1].setCurrentVelocity(vel(1)); + } + Vector2f getCurrentVelocity() const { return _state.v; } + void setCurrentPosition(const Vector2f pos) { + _state.x = pos; + _smoothing[0].setCurrentPosition(pos(0)); + _smoothing[1].setCurrentPosition(pos(1)); + } + Vector2f getCurrentPosition() const { return _position_setpoint_locked; } + void setCurrentPositionEstimate(Vector2f pos) { _position_estimate = pos; } + +private: + void resetPositionLock(); + void updateTrajectories(float dt); + void updateTrajConstraints(); + void checkPositionLock(Vector2f velocity_target); + void updateTrajDurations(Vector2f velocity_target); + + VelocitySmoothing _smoothing[2]; ///< Smoothing in x and y directions + + bool _position_lock_active{false}; + + Vector2f _position_setpoint_locked; + + Vector2f _velocity_setpoint_feedback; + Vector2f _position_estimate; + + struct { + Vector2f j; + Vector2f a; + Vector2f v; + Vector2f x; + } _state; +}; diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp new file mode 100644 index 000000000000..187bcdd724db --- /dev/null +++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "ManualVelocitySmoothingZ.hpp" + +#include +#include + +void ManualVelocitySmoothingZ::reset(float accel, float vel, float pos) +{ + _smoothing.reset(accel, vel, pos); + + resetPositionLock(); +} +void ManualVelocitySmoothingZ::resetPositionLock() +{ + _position_lock_active = false; + _position_setpoint_locked = NAN; +} + +void ManualVelocitySmoothingZ::update(float dt, float velocity_target) +{ + // Update state + updateTrajectories(dt); + + // Set max accel/vel/jerk + // Has to be done before _updateTrajDurations() + updateTrajConstraints(velocity_target); + + // Lock or unlock position + // Has to be done before _updateTrajDurations() + checkPositionLock(velocity_target); + + // Update durations + _smoothing.updateDurations(velocity_target); +} + +void ManualVelocitySmoothingZ::updateTrajectories(float dt) +{ + _smoothing.updateTraj(dt); + + _state.j = _smoothing.getCurrentJerk(); + _state.a = _smoothing.getCurrentAcceleration(); + _state.v = _smoothing.getCurrentVelocity(); + _state.x = _smoothing.getCurrentPosition(); +} + +void ManualVelocitySmoothingZ::updateTrajConstraints(float velocity_target) +{ + if (velocity_target < 0.f) { // up + _smoothing.setMaxAccel(_max_accel_up); + _smoothing.setMaxVel(_max_vel_up); + + } else { // down + _smoothing.setMaxAccel(_max_accel_down); + _smoothing.setMaxVel(_max_vel_down); + } +} + +void ManualVelocitySmoothingZ::checkPositionLock(float velocity_target) +{ + /** + * During a position lock -> position unlock transition, we have to make sure that the velocity setpoint + * is continuous. We know that the output of the position loop (part of the velocity setpoint) + * will suddenly become null + * and only the feedforward (generated by this flight task) will remain. + * This is why the previous input of the velocity controller + * is used to set current velocity of the trajectory. + */ + if (fabsf(_state.v) < 0.1f && + fabsf(_state.a) < .2f && + fabsf(velocity_target) <= FLT_EPSILON) { + // Lock position + _position_lock_active = true; + _position_setpoint_locked = _state.x; + + } else { + // Unlock position + if (_position_lock_active) { + // Start the trajectory at the current velocity setpoint + _smoothing.setCurrentVelocity(_velocity_setpoint_feedback); + _position_setpoint_locked = NAN; + } + + _position_lock_active = false; + _smoothing.setCurrentPosition(_position_estimate); + } +} diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp new file mode 100644 index 000000000000..7d96803ee743 --- /dev/null +++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 ManualVelocitySmoothingZ.hpp + * + */ + +#pragma once + +#include "VelocitySmoothing.hpp" + +class ManualVelocitySmoothingZ +{ +public: + ManualVelocitySmoothingZ() = default; + + virtual ~ManualVelocitySmoothingZ() = default; + + void reset(float accel, float vel, float pos); + void update(float dt, float velocity_target); + + void setVelSpFeedback(const float fb) { _velocity_setpoint_feedback = fb; } + + void setMaxJerk(const float max_jerk) { + _smoothing.setMaxJerk(max_jerk); + } + void setMaxAccelUp(const float max_accel_up) { + _max_accel_up = max_accel_up; + } + void setMaxAccelDown(const float max_accel_down) { + _max_accel_down = max_accel_down; + } + void setMaxVelUp(const float max_vel_up) { + _max_vel_up = max_vel_up; + } + void setMaxVelDown(const float max_vel_down) { + _max_vel_down = max_vel_down; + } + + float getCurrentJerk() const { return _state.j; } + float getCurrentAcceleration() const { return _state.a; } + void setCurrentVelocity(const float vel) { + _state.v = vel; + _smoothing.setCurrentVelocity(vel); + } + float getCurrentVelocity() const { return _state.v; } + void setCurrentPosition(const float pos) { + _state.x = pos; + _smoothing.setCurrentPosition(pos); + } + float getCurrentPosition() const { return _position_setpoint_locked; } + void setCurrentPositionEstimate(float pos) { _position_estimate = pos; } + +private: + void resetPositionLock(); + void updateTrajectories(float dt); + void updateTrajConstraints(float vel_target); + void checkPositionLock(float velocity_target); + void updateTrajDurations(float velocity_target); + + VelocitySmoothing _smoothing; ///< Smoothing in z direction + + bool _position_lock_active{false}; + + float _position_setpoint_locked; + + float _velocity_setpoint_feedback; + float _position_estimate; + + struct { + float j; + float a; + float v; + float x; + } _state; + + float _max_accel_up{0.f}; + float _max_accel_down{0.f}; + float _max_vel_up{0.f}; + float _max_vel_down{0.f}; +};