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:
Matthias Grob committed 2026-11-12 19:56:01 +01:00
1 parent 85228f2111
commit 8e94784e1d
11 files changed
+14 -330

No files matched your search

+1 -2
View File
@@ -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{};
};
@@ -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};