fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 00:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Separate RC and manual control terms

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2025-08-15 13:57:31 +02:00
committed by Matthias Grob
parent e15752099f
commit 16f97635ce
9 changed files with 42 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+1 -1
View File
@@ -289,7 +289,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
} else {
// Make sure we have a valid land position even in the case we loose RC while amending it
// Make sure we have a valid land position even in the case we loose manual control while amending it
if (!PX4_ISFINITE(_land_position(0))) {
_land_position.xy() = Vector2f(_position);
}
src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
+5 -5
View File
@@ -144,7 +144,7 @@ void FlightTaskAutoFollowTarget::updateRcAdjustedFollowHeight(const Sticks &stic
{
// Only apply Follow height adjustment if height setpoint and current height are within time window
if (fabsf(_position_setpoint(2) - _position(2)) < FOLLOW_HEIGHT_USER_ADJUST_SPEED * USER_ADJUSTMENT_ERROR_TIME_WINDOW) {
// RC Throttle stick input for changing follow height
// Manual Throttle stick input for changing follow height
const float height_change_speed = FOLLOW_HEIGHT_USER_ADJUST_SPEED * sticks.getThrottleZeroCenteredExpo();
const float new_height = _follow_height + height_change_speed * _deltatime;
_follow_height = constrain(new_height, MINIMUM_SAFETY_ALTITUDE, FOLLOW_HEIGHT_MAX);
@@ -157,7 +157,7 @@ void FlightTaskAutoFollowTarget::updateRcAdjustedFollowDistance(const Sticks &st
// Only apply Follow distance adjustment if distance setting and current distance are within time window
if (fabsf(drone_to_target_vector.length() - _follow_distance) < FOLLOW_DISTANCE_USER_ADJUST_SPEED *
USER_ADJUSTMENT_ERROR_TIME_WINDOW) {
// RC Pitch stick input for changing distance
// Manual Pitch stick input for changing distance
const float distance_change_speed = FOLLOW_DISTANCE_USER_ADJUST_SPEED * sticks.getPitchExpo();
const float new_distance = _follow_distance + distance_change_speed * _deltatime;
_follow_distance = constrain(new_distance, MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL, FOLLOW_DISTANCE_MAX);
@@ -171,9 +171,9 @@ void FlightTaskAutoFollowTarget::updateRcAdjustedFollowAngle(const Sticks &stick
// Wrap orbit angle difference, to get the shortest angle between them
if (fabsf(matrix::wrap_pi(measured_orbit_angle - tracked_orbit_angle_setpoint)) < FOLLOW_ANGLE_USER_ADJUST_SPEED *
USER_ADJUSTMENT_ERROR_TIME_WINDOW) {
// RC Roll stick input for changing follow angle. When user commands RC stick input: +Roll (right), angle increases (clockwise)
// Manual Roll stick input for changing follow angle. When user commands manual stick input: +Roll (right), angle increases (clockwise)
// Constrain adjust speed [rad/s] so that drone can actually catch up. Otherwise, the follow angle
// command can be too ahead that drone's behavior would be un-responsive to RC stick inputs.
// command can be too ahead that drone's behavior would be un-responsive to manual stick inputs.
const float angle_adjust_speed_max = min(FOLLOW_ANGLE_USER_ADJUST_SPEED,
_param_flw_tgt_max_vel.get() / _follow_distance);
const float angle_change_speed = angle_adjust_speed_max * sticks.getRollExpo();
@@ -319,7 +319,7 @@ bool FlightTaskAutoFollowTarget::update()
// Actual orbit angle measured around the target, which is pointing from target to drone, so M_PI_F difference.
const float measured_orbit_angle = matrix::wrap_pi(drone_to_target_heading + M_PI_F);
// Update the sticks object to fetch recent data and update follow distance, angle and height via RC commands
// Update the sticks object to fetch recent data and update follow distance, angle and height via manual commands
_sticks.checkAndUpdateStickInputs();
if (_sticks.isAvailable()) {
src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp
+17 -17
View File
@@ -101,26 +101,26 @@ static constexpr float ORBIT_TRAJECTORY_MAX_JERK = 4.0;
// [m/s^2] Maximum acceleration setting for generating the Follow Target Orbit trajectory
static constexpr float ORBIT_TRAJECTORY_MAX_ACCELERATION = 2.0;
// << RC Adjustment related constants >>
// << manual Adjustment related constants >>
// [m/s] Speed with which the follow distance will be adjusted by when commanded with deflection via RC command
// [m/s] Speed with which the follow distance will be adjusted by when commanded with deflection via sticks
static constexpr float FOLLOW_DISTANCE_USER_ADJUST_SPEED = 2.0;
// [m] Maximum follow distance that can be set by user's RC adjustment
// [m] Maximum follow distance that can be set by user's manual adjustment
static constexpr float FOLLOW_DISTANCE_MAX = 100.f;
// [m/s] Speed with which the follow height will be adjusted by when commanded with deflection via RC command
// [m/s] Speed with which the follow height will be adjusted by when commanded with deflection via sticks
static constexpr float FOLLOW_HEIGHT_USER_ADJUST_SPEED = 1.5;
// [m] Maximum follow height that can be set by user's RC adjustment
// [m] Maximum follow height that can be set by user's manual adjustment
static constexpr float FOLLOW_HEIGHT_MAX = 100.f;
// [rad/s] Angular rate with which the follow distance will be adjusted by when commanded with full deflection via RC command
// [rad/s] Angular rate with which the follow distance will be adjusted by when commanded with full deflection via sticks
static constexpr float FOLLOW_ANGLE_USER_ADJUST_SPEED = 1.5;
// [s] Time window constant that gets multiplied to user adjustment speed, to calculate the
// 'acceptable' error in orbit angle / height / distance. If the difference between the setpoint
// and actual state of the drone is smaller than this error, RC adjustments get applied.
// and actual state of the drone is smaller than this error, manual adjustments get applied.
// Prevents setpoints diverging from the vehicle's actual position too much
static constexpr float USER_ADJUSTMENT_ERROR_TIME_WINDOW = 0.5f;
@@ -146,33 +146,33 @@ protected:
};
/**
* Update the Follow height based on RC commands
* Update the Follow height based on stick inputs
*
* If the drone's height error to the setpoint is within the an user adjustment error time window
* follow_height will be adjusted with a speed proportional to user RC command
* follow_height will be adjusted with a speed proportional to user stick inputs
*
* @param sticks Sticks object to get RC commanded values for adjustments
* @param sticks Sticks object to get manually values for adjustments
*/
void updateRcAdjustedFollowHeight(const Sticks &sticks);
/**
* Update the Follow distance based on RC commands
* Update the Follow distance based on stick inputs
*
* If the drone's distance error to the setpoint is within the an user adjustment error time window
* follow_distance will be adjusted with a speed proportional to user RC command
* follow_distance will be adjusted with a speed proportional to user stick inputs
*
* @param sticks Sticks object to get RC commanded values for adjustments
* @param sticks Sticks object to get manually values for adjustments
* @param drone_to_target_vector [m] Tracked follow distance variable reference which will be updated to the new value
*/
void updateRcAdjustedFollowDistance(const Sticks &sticks, const Vector2f &drone_to_target_vector);
/**
* Update the Follow angle based on RC commands
* Update the Follow angle based on stick inputs
*
* If the drone's orbit angle in relation to target is within the an user adjustment error time window
* away from the orbit angle setpoint, follow_angle will be adjusted with a speed proportional to user RC command
* away from the orbit angle setpoint, follow_angle will be adjusted with a speed proportional to user stick inputs
*
* @param sticks Sticks object to get RC commanded values for adjustments
* @param sticks Sticks object to get manually values for adjustments
* @param measured_angle [rad] Measured current drone's orbit angle around the target (depends on tracked target orientation for reference)
* @param tracked_orbit_angle_setpoint [rad] Rate constrained orbit angle setpoint value from last command
*/
@@ -275,7 +275,7 @@ protected:
// Second Order Filter to calculate kinematically feasible target position
SecondOrderReferenceModel<matrix::Vector3f> _target_position_velocity_filter;
// Internally tracked Follow Target characteristics, to allow RC control input adjustments
// Internally tracked Follow Target characteristics, to allow manual control input adjustments
float _follow_distance{8.0f}; // [m]
float _follow_height{10.0f}; // [m]
float _follow_angle_rad{0.0f}; // [rad]
src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c
+1 -1
View File
@@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(MC_ORBIT_RAD_MAX, 1000.0f);
* @value 1 Hold Initial Heading
* @value 2 Uncontrolled
* @value 3 Hold Front Tangent to Circle
* @value 4 RC Controlled
* @value 4 Manually (yaw stick) Controlled
* @group Flight Task Orbit
*/
PARAM_DEFINE_INT32(MC_ORBIT_YAW_MOD, 0);