mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
refactor(flight_mode_manager): remove unused code
ManualVelocitySmoothingXY has had no user since FlightTaskManualPositionSmoothVel was removed. Its position lock test after a reset moves to the still used ManualVelocitySmoothingZ. Also remove members and methods of the flight tasks that are never used: the acceleration setpoint feedback and the activation timestamp were only written, updateVelocityControllerFeedback(), errorToString() and StickAccelerationXY::getVelocityConstraint() were never called, and _accelerationValid(), setGearAccordingToSwitch() and _updateYawCorrection() were declared without a definition.
This commit is contained in:
1 parent
85228f2111
commit
8e94784e1d
11 files changed
+14
-330
No files matched your search
@@ -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)
|
||||
@@ -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 <mathlib/mathlib.h>
|
||||
#include <float.h>
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
@@ -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 <motion_planning/VelocitySmoothing.hpp>
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
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{};
|
||||
};
|
||||
+13
-57
@@ -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 <gtest/gtest.h>
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
#include <motion_planning/ManualVelocitySmoothingXY.hpp>
|
||||
#include <motion_planning/ManualVelocitySmoothingZ.hpp>
|
||||
|
||||
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);
|
||||
}
|
||||
@@ -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();
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
|
||||
uORB::SubscriptionData<home_position_s> _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{};
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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};
|
||||
|
||||
Reference in new issue
Block a user