diff --git a/src/lib/motion_planning/CMakeLists.txt b/src/lib/motion_planning/CMakeLists.txt index 54d396697e1..bcaa900ed8b 100644 --- a/src/lib/motion_planning/CMakeLists.txt +++ b/src/lib/motion_planning/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_library(motion_planning HeadingSmoothing.cpp - ManualVelocitySmoothingXY.cpp ManualVelocitySmoothingZ.cpp PositionSmoothing.cpp VelocitySmoothing.cpp @@ -44,5 +43,5 @@ target_include_directories(motion_planning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS motion_planning) px4_add_unit_gtest(SRC PositionSmoothingTest.cpp LINKLIBS motion_planning) px4_add_unit_gtest(SRC HeadingSmoothingTest.cpp LINKLIBS motion_planning) -px4_add_unit_gtest(SRC ManualVelocitySmoothingXYTest.cpp LINKLIBS motion_planning) +px4_add_unit_gtest(SRC ManualVelocitySmoothingZTest.cpp LINKLIBS motion_planning) px4_add_unit_gtest(SRC TrajectoryConstraintsTest.cpp LINKLIBS motion_planning) diff --git a/src/lib/motion_planning/ManualVelocitySmoothingXY.cpp b/src/lib/motion_planning/ManualVelocitySmoothingXY.cpp deleted file mode 100644 index 914e43e0a74..00000000000 --- a/src/lib/motion_planning/ManualVelocitySmoothingXY.cpp +++ /dev/null @@ -1,110 +0,0 @@ -/**************************************************************************** - * - * 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(const Vector2f &accel, const Vector2f &vel, const Vector2f &pos) -{ - for (int i = 0; i < 2; i++) { - _trajectory[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, const Vector2f &velocity_target) -{ - // Lock or unlock position - // Has to be done before _updateTrajDurations() - checkPositionLock(velocity_target); - - // Update durations and sync XY - updateTrajDurations(velocity_target); - - // Integrate along the plan - for (int i = 0; i < 2; ++i) { - _trajectory[i].updateTraj(dt); - } -} - -void ManualVelocitySmoothingXY::updateTrajDurations(const Vector2f &velocity_target) -{ - for (int i = 0; i < 2; ++i) { - _trajectory[i].updateDurations(velocity_target(i)); - } - - VelocitySmoothing::timeSynchronization(_trajectory, 2); -} - -void ManualVelocitySmoothingXY::checkPositionLock(const 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 (getCurrentVelocity().length() < 0.1f && - getCurrentAcceleration().length() < .2f && - velocity_target.length() <= FLT_EPSILON) { - // Lock position - _position_lock_active = true; - _position_setpoint_locked = Vector2f(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition()); - - } else { - // Unlock position - if (_position_lock_active) { - // Start the trajectory at the current velocity setpoint - _trajectory[0].setCurrentVelocity(_velocity_setpoint_feedback(0)); - _trajectory[1].setCurrentVelocity(_velocity_setpoint_feedback(1)); - resetPositionLock(); - } - - _trajectory[0].setCurrentPosition(_position_estimate(0)); - _trajectory[1].setCurrentPosition(_position_estimate(1)); - } -} diff --git a/src/lib/motion_planning/ManualVelocitySmoothingXY.hpp b/src/lib/motion_planning/ManualVelocitySmoothingXY.hpp deleted file mode 100644 index 7048bf36c22..00000000000 --- a/src/lib/motion_planning/ManualVelocitySmoothingXY.hpp +++ /dev/null @@ -1,116 +0,0 @@ -/**************************************************************************** - * - * 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 - -#include - -using matrix::Vector2f; - -class ManualVelocitySmoothingXY final -{ -public: - ManualVelocitySmoothingXY() = default; - ~ManualVelocitySmoothingXY() = default; - - void reset(const Vector2f &accel, const Vector2f &vel, const Vector2f &pos); - void update(float dt, const Vector2f &velocity_target); - - void setVelSpFeedback(const Vector2f &fb) { _velocity_setpoint_feedback = fb; } - - void setMaxJerk(const float max_jerk) - { - _trajectory[0].setMaxJerk(max_jerk); - _trajectory[1].setMaxJerk(max_jerk); - } - float getMaxJerk() const { return _trajectory[0].getMaxJerk(); } - - void setMaxAccel(const float max_accel) - { - _trajectory[0].setMaxAccel(max_accel); - _trajectory[1].setMaxAccel(max_accel); - } - float getMaxAccel() const { return _trajectory[0].getMaxAccel(); } - - void setMaxVel(const float max_vel) - { - _trajectory[0].setMaxVel(max_vel); - _trajectory[1].setMaxVel(max_vel); - } - float getMaxVel() const { return _trajectory[0].getMaxVel(); } - - Vector2f getCurrentJerk() const { return {_trajectory[0].getCurrentJerk(), _trajectory[1].getCurrentJerk()}; } - Vector2f getCurrentAcceleration() const { return {_trajectory[0].getCurrentAcceleration(), _trajectory[1].getCurrentAcceleration()}; } - - void setCurrentVelocity(const Vector2f &vel) - { - _trajectory[0].setCurrentVelocity(vel(0)); - _trajectory[1].setCurrentVelocity(vel(1)); - } - Vector2f getCurrentVelocity() const { return {_trajectory[0].getCurrentVelocity(), _trajectory[1].getCurrentVelocity()}; } - - void setCurrentPosition(const Vector2f &pos) - { - _trajectory[0].setCurrentPosition(pos(0)); - _trajectory[1].setCurrentPosition(pos(1)); - _position_estimate = pos; - - if (_position_lock_active) { - _position_setpoint_locked = pos; - } - } - Vector2f getCurrentPosition() const { return _position_setpoint_locked; } - - void setCurrentPositionEstimate(const Vector2f &pos) { _position_estimate = pos; } - -private: - void resetPositionLock(); - void checkPositionLock(const Vector2f &velocity_target); - void updateTrajDurations(const Vector2f &velocity_target); - - VelocitySmoothing _trajectory[2]; ///< Trajectory in x and y directions - - bool _position_lock_active{false}; - - Vector2f _position_setpoint_locked{}; - - Vector2f _velocity_setpoint_feedback{}; - Vector2f _position_estimate{}; -}; diff --git a/src/lib/motion_planning/ManualVelocitySmoothingXYTest.cpp b/src/lib/motion_planning/ManualVelocitySmoothingZTest.cpp similarity index 53% rename from src/lib/motion_planning/ManualVelocitySmoothingXYTest.cpp rename to src/lib/motion_planning/ManualVelocitySmoothingZTest.cpp index 351788c067c..3c333d1c8e7 100644 --- a/src/lib/motion_planning/ManualVelocitySmoothingXYTest.cpp +++ b/src/lib/motion_planning/ManualVelocitySmoothingZTest.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Copyright (C) 2026 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 @@ -37,68 +37,24 @@ */ #include -#include -#include +#include -using namespace matrix; - -class ManualVelocitySmoothingXYTest : public ::testing::Test -{ -public: - ManualVelocitySmoothingXY _smoothing; -}; - - -TEST_F(ManualVelocitySmoothingXYTest, setGet) -{ - // GIVEN: Some max values - _smoothing.setMaxJerk(11.f); - _smoothing.setMaxAccel(7.f); - _smoothing.setMaxVel(5.f); - - // THEN: We should be able to get them back - EXPECT_FLOAT_EQ(_smoothing.getMaxJerk(), 11.f); - EXPECT_FLOAT_EQ(_smoothing.getMaxAccel(), 7.f); - EXPECT_FLOAT_EQ(_smoothing.getMaxVel(), 5.f); -} - -TEST_F(ManualVelocitySmoothingXYTest, getCurrentState) -{ - // GIVEN: the initial conditions - Vector2f v0(11.f, 13.f); - _smoothing.setCurrentVelocity(v0); - - // WHEN: we get the current state - Vector3f j_end; - Vector3f a_end; - Vector3f v_end; - Vector3f x_end; - j_end.xy() = _smoothing.getCurrentJerk(); - a_end.xy() = _smoothing.getCurrentAcceleration(); - v_end.xy() = _smoothing.getCurrentVelocity(); - x_end.xy() = _smoothing.getCurrentPosition(); - - // THEN: the returned values should match the input - EXPECT_EQ(j_end, Vector3f(0.f, 0.f, 0.f)); - EXPECT_EQ(a_end, Vector3f(0.f, 0.f, 0.f)); - EXPECT_EQ(v_end, Vector3f(11.f, 13.f, 0.f)); - EXPECT_EQ(x_end, Vector3f(0.f, 0.f, 0.f)); -} - -TEST_F(ManualVelocitySmoothingXYTest, positionLockAfterReset) +TEST(ManualVelocitySmoothingZTest, positionLockAfterReset) { // GIVEN: A standing still trajectory reset away from the origin - _smoothing.setMaxJerk(4.f); - _smoothing.setMaxAccel(3.f); - _smoothing.setMaxVel(5.f); - const Vector2f position(10.f, -5.f); - _smoothing.reset(Vector2f(), Vector2f(), position); + ManualVelocitySmoothingZ smoothing; + smoothing.setMaxJerk(4.f); + smoothing.setMaxAccelUp(3.f); + smoothing.setMaxAccelDown(3.f); + smoothing.setMaxVelUp(3.f); + smoothing.setMaxVelDown(3.f); + const float position = -10.f; + smoothing.reset(0.f, 0.f, position); // WHEN: It's updated without any velocity target - _smoothing.update(0.02f, Vector2f()); + smoothing.update(0.02f, 0.f); // THEN: The position gets locked where it was reset to - EXPECT_FLOAT_EQ(_smoothing.getCurrentPosition()(0), position(0)); - EXPECT_FLOAT_EQ(_smoothing.getCurrentPosition()(1), position(1)); + EXPECT_FLOAT_EQ(smoothing.getCurrentPosition(), position); } diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 94d61d701a1..532c76a0487 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -428,19 +428,6 @@ FlightTaskError FlightModeManager::switchTask(int new_task_index) return FlightTaskError::InvalidTask; } -const char *FlightModeManager::errorToString(const FlightTaskError error) -{ - switch (error) { - case FlightTaskError::NoError: return "No Error"; - - case FlightTaskError::InvalidTask: return "Invalid Task"; - - case FlightTaskError::ActivationFailed: return "Activation Failed"; - } - - return "This error is not mapped to a string or is unknown."; -} - int FlightModeManager::task_spawn(int argc, char *argv[]) { FlightModeManager *instance = new FlightModeManager(); diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index 2e2d2184128..063710cf989 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -102,11 +102,6 @@ private: FlightTaskError switchTask(FlightTaskIndex new_task_index); FlightTaskError switchTask(int new_task_index); - /** - * Call this method to get the description of a task error. - */ - const char *errorToString(const FlightTaskError error); - /** * Check if any task is active * @return true if a task is active, false if not diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index 805ac3c672d..c56ada38344 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -12,7 +12,6 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint) _resetSetpoints(); _setDefaultConstraints(); initEkfResetCounters(); - _time_stamp_activate = hrt_absolute_time(); _gear = empty_landing_gear_default_keep; return true; } @@ -197,11 +196,9 @@ void FlightTask::_evaluateVehicleLocalPositionSetpoint() // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock) _velocity_setpoint_feedback = matrix::Vector3f(vehicle_local_position_setpoint.vx, vehicle_local_position_setpoint.vy, vehicle_local_position_setpoint.vz); - _acceleration_setpoint_feedback = matrix::Vector3f(vehicle_local_position_setpoint.acceleration); } else { _velocity_setpoint_feedback.setAll(NAN); - _acceleration_setpoint_feedback.setAll(NAN); } } diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 080c594d1ab..8dd954044b4 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -147,13 +147,6 @@ public: virtual void overrideCruiseSpeed(const float cruise_speed_m_s) {} - void updateVelocityControllerFeedback(const matrix::Vector3f &vel_sp, - const matrix::Vector3f &acc_sp) - { - _velocity_setpoint_feedback = vel_sp; - _acceleration_setpoint_feedback = acc_sp; - } - protected: uORB::SubscriptionData _sub_vehicle_local_position{ORB_ID(vehicle_local_position)}; uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; @@ -194,7 +187,6 @@ protected: float _deltatime{}; /**< passed time in seconds since the task was last updated */ - hrt_abstime _time_stamp_activate{}; /**< time stamp when task was activated */ hrt_abstime _time_stamp_current{}; /**< time stamp at the beginning of the current task update */ hrt_abstime _time_stamp_last{}; /**< time stamp when task was last updated */ @@ -221,7 +213,6 @@ protected: matrix::Vector3f _velocity_setpoint; matrix::Vector3f _velocity_setpoint_feedback; matrix::Vector3f _acceleration_setpoint; - matrix::Vector3f _acceleration_setpoint_feedback; matrix::Vector3f _jerk_setpoint; float _yaw_setpoint{}; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 9848fdff65a..920731ee2b0 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -122,10 +122,6 @@ private: */ void _respectGroundSlowdown(); - void setGearAccordingToSwitch(); - - bool _updateYawCorrection(); - uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ float _min_distance_to_ground{(float)(-INFINITY)}; /**< min distance to ground constraint */ diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index b23adb113ab..c98b9fbd58a 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -75,16 +75,6 @@ private: static constexpr float _acceleration_max = 2.f; static constexpr float _horizontal_acceptance_radius = 2.f; - /** - * Check the feasibility of orbit parameters with respect to - * centripetal acceleration a = v^2 / r - * @param radius desired radius - * @param velocity desired velocity - * @param acceleration maximal allowed acceleration - * @return true on success, false if value not accepted - */ - bool _accelerationValid(float radius, float velocity, float acceleration) const; - /** * Checks if desired orbit params are feasible. If not, * params are modified such that it is possible diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 50170535786..5053d54eee8 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -67,7 +67,6 @@ public: // Assuming the velocity constraint resets in every loop and update constraint if new value is lower void setVelocityConstraint(float vel); - float getVelocityConstraint() { return _velocity_slew_rate_xy.getState(); }; private: CollisionPrevention _collision_prevention{this};