fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp
View File

@ -37,7 +37,6 @@ using namespace time_literals;
void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporter)
{
// RC
manual_control_setpoint_s manual_control_setpoint;
if (!_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
@ -45,7 +44,7 @@ void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporte
reporter.failsafeFlags().manual_control_signal_lost = true;
}
// Check if RC is valid
// Check if manual control is valid
if (!manual_control_setpoint.valid
|| hrt_elapsed_time(&manual_control_setpoint.timestamp) > _param_com_rc_loss_t.get() * 1_s) {
@ -77,14 +76,14 @@ void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporte
if (reporter.failsafeFlags().gcs_connection_lost) {
// Prevent arming if we neither have RC nor a GCS connection. TODO: disabled for now due to MAVROS tests
// Prevent arming if we neither have manual control nor a GCS connection. TODO: disabled for now due to MAVROS tests
bool gcs_connection_required = _param_nav_dll_act.get() > 0
/*|| (rc_is_optional && reporter.failsafeFlags().manual_control_signal_lost) */;
NavModes affected_modes = gcs_connection_required ? NavModes::All : NavModes::None;
events::LogLevel log_level = gcs_connection_required ? events::Log::Error : events::Log::Info;
/* EVENT
* @description
* To arm, at least a data link or manual control (RC) must be present.
* To arm, at least a data link or RC must be present.
*
* <profile name="dev">
* This check can be configured via <param>NAV_DLL_ACT</param> parameter.

17
src/modules/commander/commander_params.c
View File

@ -190,7 +190,7 @@ PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0);
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 3);
/**
* RC input arm/disarm command duration
* Manual control input arm/disarm command duration
*
* The default value of 1000 requires the stick to be held in the arm or disarm position for 1 second.
*
@ -421,9 +421,9 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 60);
PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 2);
/**
* Enable RC stick override of auto and/or offboard modes
* Enable manual control stick override
*
* When RC stick override is enabled, moving the RC sticks more than COM_RC_STICK_OV
* When enabled, moving the sticks more than COM_RC_STICK_OV
* immediately gives control back to the pilot by switching to Position mode and
* if position is unavailable Altitude mode.
* Note: Only has an effect on multicopters, and VTOLs in multicopter mode.
@ -437,7 +437,7 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 2);
PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1);
/**
* RC stick override threshold
* Stick override threshold
*
* If COM_RC_OVERRIDE is enabled and the joystick input is moved more than this threshold
* the autopilot the pilot takes over control.
@ -601,11 +601,10 @@ PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
/**
* Set RC loss failsafe mode
* Set manual control loss failsafe mode
*
* The RC loss failsafe will only be entered after a timeout,
* set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled
* by setting the COM_RC_IN_MODE param it will not be triggered.
* The manual control loss failsafe will only be entered after a timeout,
* set by COM_RC_LOSS_T in seconds.
*
* @value 1 Hold mode
* @value 2 Return mode
@ -620,7 +619,7 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
/**
* RC loss exceptions
* Manual control loss exceptions
*
* Specify modes where manual control loss is ignored and no failsafe is triggered.
* External modes requiring stick input will still failsafe.

2
src/modules/commander/failsafe/failsafe.cpp
View File

@ -456,7 +456,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
&& in_forward_flight && !state.mission_finished;
// Manual control (RC) loss
// Manual control (RC or joystick) loss
if (!status_flags.manual_control_signal_lost) {
// If manual control was lost and arming was allowed, consider it optional until we regain manual control
_manual_control_lost_at_arming = false;

8
src/modules/commander/failsafe/failsafe_test.cpp
View File

@ -109,7 +109,7 @@ TEST_F(FailsafeTest, general)
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
// RC lost -> Hold, then RTL
// manual control lost -> Hold, then RTL
time += 10_ms;
failsafe_flags.manual_control_signal_lost = true;
updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
@ -127,14 +127,14 @@ TEST_F(FailsafeTest, general)
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Descend);
// DL link regained -> RTL (RC still lost)
// DL link regained -> RTL (manual control still lost)
time += 10_ms;
failsafe_flags.gcs_connection_lost = false;
updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
// RC lost cleared -> keep RTL
// Manual control lost cleared -> keep RTL
time += 10_ms;
failsafe_flags.manual_control_signal_lost = false;
updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
@ -425,7 +425,7 @@ TEST_F(FailsafeTest, skip_failsafe)
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
// RC lost while in RTL -> stay in RTL and only warn
// Manual control lost while in RTL -> stay in RTL and only warn
failsafe_flags.manual_control_signal_lost = true;
updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);

2
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
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);
}

10
src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
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()) {

34
src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp
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]

2
src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c
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);

4
src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c
View File

@ -105,9 +105,9 @@ PARAM_DEFINE_INT32(FW_AT_APPLY, 2);
PARAM_DEFINE_INT32(FW_AT_AXES, 3);
/**
* Enable/disable auto tuning using an RC AUX input
* Enable/disable auto tuning using a manual control AUX input
*
* Defines which RC_MAP_AUXn parameter maps the RC channel used to enable/disable auto tuning.
* Defines which RC_MAP_AUXn parameter maps the manual control channel used to enable/disable auto tuning.
*
* @value 0 Disable
* @value 1 Aux1