fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 00:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move Stick class into a globally available library

Browse Source
This commit is contained in:
mahima-yoga
2025-09-18 15:50:10 +02:00
committed by Silvan Fuhrer
parent 2cb97e79b9
commit a514289a68
15 changed files with 50 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt
+1 -1
View File
@@ -35,5 +35,5 @@ px4_add_library(FlightTaskAuto
FlightTaskAuto.cpp
)
target_link_libraries(FlightTaskAuto PUBLIC FlightTask FlightTaskUtility StickYaw WeatherVane)
target_link_libraries(FlightTaskAuto PUBLIC FlightTask FlightTaskUtility Sticks StickYaw WeatherVane)
target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
+1 -1
View File
@@ -49,9 +49,9 @@
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <lib/motion_planning/HeadingSmoothing.hpp>
#include <lib/motion_planning/PositionSmoothing.hpp>
#include <lib/sticks/Sticks.hpp>
#include <lib/stick_yaw/StickYaw.hpp>
#include <lib/weather_vane/WeatherVane.hpp>
#include "Sticks.hpp"
#include "StickAccelerationXY.hpp"
/**
src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp
+1 -1
View File
@@ -44,7 +44,6 @@
#include "FlightTaskAuto.hpp"
#include "follow_target_estimator/TargetEstimator.hpp"
#include "Sticks.hpp"
#include <parameters/param.h>
#include <mathlib/mathlib.h>
@@ -57,6 +56,7 @@
#include <uORB/topics/vehicle_command.h>
#include <lib/mathlib/math/filter/second_order_reference_model.hpp>
#include <lib/sticks/Sticks.hpp>
#include <motion_planning/VelocitySmoothing.hpp>
// << Follow Target Behavior related constants >>
src/modules/flight_mode_manager/tasks/Descend/CMakeLists.txt
+1 -1
View File
@@ -35,5 +35,5 @@ px4_add_library(FlightTaskDescend
FlightTaskDescend.cpp
)
target_link_libraries(FlightTaskDescend PUBLIC FlightTask FlightTaskUtility StickYaw)
target_link_libraries(FlightTaskDescend PUBLIC FlightTask FlightTaskUtility Sticks StickYaw)
target_include_directories(FlightTaskDescend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.hpp
+1 -1
View File
@@ -37,9 +37,9 @@
#pragma once
#include <lib/sticks/Sticks.hpp>
#include <lib/stick_yaw/StickYaw.hpp>
#include "FlightTask.hpp"
#include "Sticks.hpp"
#include "StickTiltXY.hpp"
class FlightTaskDescend : public FlightTask
src/modules/flight_mode_manager/tasks/ManualAltitude/CMakeLists.txt
+1 -1
View File
@@ -35,5 +35,5 @@ px4_add_library(FlightTaskManualAltitude
FlightTaskManualAltitude.cpp
)
target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTask FlightTaskUtility StickYaw)
target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTask FlightTaskUtility Sticks StickYaw)
target_include_directories(FlightTaskManualAltitude PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+1 -1
View File
@@ -40,8 +40,8 @@
#pragma once
#include <lib/stick_yaw/StickYaw.hpp>
#include <lib/sticks/Sticks.hpp>
#include "FlightTask.hpp"
#include "Sticks.hpp"
#include "StickTiltXY.hpp"
#include <uORB/Subscription.hpp>
src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt
+1 -2
View File
@@ -32,13 +32,12 @@
############################################################################
px4_add_library(FlightTaskUtility
Sticks.cpp
StickAccelerationXY.cpp
StickTiltXY.cpp
Gimbal.cpp
)
target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis SlewRate motion_planning mathlib)
target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis SlewRate Sticks motion_planning mathlib)
target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
px4_add_functional_gtest(SRC StickTiltXYTest.cpp LINKLIBS FlightTaskUtility)
src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp
+2 -1
View File
@@ -48,7 +48,8 @@
#include <uORB/topics/gimbal_manager_status.h>
#include <uORB/topics/vehicle_command.h>
#include "Sticks.hpp"
#include <lib/sticks/Sticks.hpp>
class Gimbal : public ModuleParams
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+1 -1
View File
@@ -34,7 +34,7 @@
#include "StickAccelerationXY.hpp"
#include <geo/geo.h>
#include "Sticks.hpp"
#include <lib/sticks/Sticks.hpp>
using namespace matrix;
src/modules/flight_mode_manager/tasks/Utility/StickTiltXY.cpp
+1 -1
View File
@@ -34,7 +34,7 @@
#include "StickTiltXY.hpp"
#include <geo/geo.h>
#include "Sticks.hpp"
#include <lib/sticks/Sticks.hpp>
using namespace matrix;
src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
-107
View File
@@ -1,107 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 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 Sticks.cpp
*/
#include "Sticks.hpp"
using namespace time_literals;
using namespace matrix;
Sticks::Sticks(ModuleParams *parent) :
ModuleParams(parent)
{}
bool Sticks::checkAndUpdateStickInputs()
{
// Sticks are rescaled linearly and exponentially to [-1,1]
manual_control_setpoint_s manual_control_setpoint;
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
// Linear scale
_positions(0) = manual_control_setpoint.pitch;
_positions(1) = manual_control_setpoint.roll;
_positions(2) = -manual_control_setpoint.throttle;
_positions(3) = manual_control_setpoint.yaw;
// Exponential scale
_positions_expo(0) = math::expo_deadzone(_positions(0), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(1) = math::expo_deadzone(_positions(1), _param_mpc_xy_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(2) = math::expo_deadzone(_positions(2), _param_mpc_z_man_expo.get(), _param_mpc_hold_dz.get());
_positions_expo(3) = math::expo_deadzone(_positions(3), _param_mpc_yaw_expo.get(), _param_mpc_hold_dz.get());
_aux_positions(0) = manual_control_setpoint.aux1;
_aux_positions(1) = manual_control_setpoint.aux2;
_aux_positions(2) = manual_control_setpoint.aux3;
_aux_positions(3) = manual_control_setpoint.aux4;
_aux_positions(4) = manual_control_setpoint.aux5;
_aux_positions(5) = manual_control_setpoint.aux6;
// valid stick inputs are required
_input_available = manual_control_setpoint.valid && _positions.isAllFinite();
} else {
failsafe_flags_s failsafe_flags;
if (_failsafe_flags_sub.update(&failsafe_flags)) {
if (failsafe_flags.manual_control_signal_lost) {
_input_available = false;
}
}
}
if (!_input_available) {
// Timeout: set all sticks to zero
_positions.zero();
_positions_expo.zero();
}
return _input_available;
}
void Sticks::limitStickUnitLengthXY(Vector2f &v)
{
const float vl = v.length();
if (vl > 1.0f) {
v /= vl;
}
}
void Sticks::rotateIntoHeadingFrameXY(Vector2f &v, const float yaw, const float yaw_setpoint)
{
const float yaw_rotate = PX4_ISFINITE(yaw_setpoint) ? yaw_setpoint : yaw;
v = Dcm2f(yaw_rotate) * v;
}
src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp
-109
View File
@@ -1,109 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020-2023 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 Sticks.hpp
*
* Library abstracting stick input from manual_control_setpoint
*
* @author Matthias Grob <maetugr@gmail.com>
*/
#pragma once
#include <px4_platform_common/module_params.h>
#include <matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/failsafe_flags.h>
class Sticks : public ModuleParams
{
public:
Sticks(ModuleParams *parent);
~Sticks() = default;
// Checks for updated manual control input & updates internal values
bool checkAndUpdateStickInputs();
bool isAvailable() { return _input_available; };
// Position : 0 : pitch, 1 : roll, 2 : throttle, 3 : yaw
const matrix::Vector4f &getPosition() { return _positions; }; // Raw stick position, no deadzone
const matrix::Vector4f &getPositionExpo() { return _positions_expo; }; // Deadzone and expo applied
// Helper functions to get stick values more intuitively
float getRoll() const { return _positions(1); }
float getRollExpo() const { return _positions_expo(1); }
float getPitch() const { return _positions(0); }
float getPitchExpo() const { return _positions_expo(0); }
float getYaw() const { return _positions(3); }
float getYawExpo() const { return _positions_expo(3); }
float getThrottleZeroCentered() const { return -_positions(2); } // Convert Z-axis(down) command to Up-axis frame
float getThrottleZeroCenteredExpo() const { return -_positions_expo(2); }
const matrix::Vector2f getPitchRoll() { return _positions.slice<2, 1>(0, 0); }
const matrix::Vector2f getPitchRollExpo() { return _positions_expo.slice<2, 1>(0, 0); }
const matrix::Vector<float, 6> &getAux() const { return _aux_positions; }
/**
* Limit the the horizontal input from a square shaped joystick gimbal to a unit circle
* @param v Vector containing x, y, z axis of the joystick gimbal. x, y get adjusted
*/
static void limitStickUnitLengthXY(matrix::Vector2f &v);
/**
* Rotate horizontal component of joystick input into the vehicle body orientation
* @param v Vector containing x, y, z axis of the joystick input.
* @param yaw Current vehicle yaw heading
* @param yaw_setpoint Current yaw setpoint if it's locked else NAN
*/
static void rotateIntoHeadingFrameXY(matrix::Vector2f &v, const float yaw, const float yaw_setpoint);
private:
bool _input_available{false};
matrix::Vector4f _positions; ///< unmodified manual stick inputs that usually move vehicle in x, y, z and yaw direction
matrix::Vector4f _positions_expo; ///< modified manual sticks using expo function
matrix::Vector<float, 6> _aux_positions;
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz,
(ParamFloat<px4::params::MPC_XY_MAN_EXPO>) _param_mpc_xy_man_expo,
(ParamFloat<px4::params::MPC_Z_MAN_EXPO>) _param_mpc_z_man_expo,
(ParamFloat<px4::params::MPC_YAW_EXPO>) _param_mpc_yaw_expo
)
};