mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 02:55:07 +08:00
Separate RC and manual control terms
Build all targets / Scan for Board Targets (push) Has been cancelled
Build all targets / Build Group [${{ matrix.group }}][${{ matrix.arch == 'nuttx' && 'x86' || 'arm64' }}] (push) Has been cancelled
Build all targets / Upload Artifacts to S3 (push) Has been cancelled
Build all targets / Create Release and Upload Artifacts (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_fmu-v5_default) (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_sitl_default) (push) Has been cancelled
Checks / build (check_format) (push) Has been cancelled
Checks / build (check_newlines) (push) Has been cancelled
Checks / build (module_documentation) (push) Has been cancelled
Checks / build (px4_fmu-v2_default stack_check) (push) Has been cancelled
Checks / build (px4_sitl_allyes) (push) Has been cancelled
Checks / build (shellcheck_all) (push) Has been cancelled
Checks / build (tests) (push) Has been cancelled
Checks / build (tests_coverage) (push) Has been cancelled
Checks / build (validate_module_configs) (push) Has been cancelled
Clang Tidy / build (push) Has been cancelled
MacOS build / build (px4_fmu-v5_default) (push) Has been cancelled
MacOS build / build (px4_sitl) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:22.04) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:24.04) (push) Has been cancelled
Container build / Set Tags and Variables (push) Has been cancelled
Container build / Build Container (amd64) (push) Has been cancelled
Container build / Build Container (arm64) (push) Has been cancelled
Container build / Deploy To Registry (push) Has been cancelled
EKF Update Change Indicator / unit_tests (push) Has been cancelled
Failsafe Simulator Build / build (failsafe_web) (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v5x (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v6x (push) Has been cancelled
FLASH usage analysis / Publish Results (push) Has been cancelled
ITCM check / Checking nxp_tropic-community (push) Has been cancelled
ITCM check / Checking px4_fmu-v5x (push) Has been cancelled
ITCM check / Checking px4_fmu-v6xrt (push) Has been cancelled
MAVROS Mission Tests / build (map[mission:MC_mission_box vehicle:iris]) (push) Has been cancelled
MAVROS Offboard Tests / build (map[test_file:mavros_posix_tests_offboard_posctl.test vehicle:iris]) (push) Has been cancelled
Nuttx Target with extra env config / build (px4_fmu-v5_default) (push) Has been cancelled
Python CI Checks / build (push) Has been cancelled
ROS Integration Tests / build (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:humble ubuntu:jammy]) (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:jazzy ubuntu:noble]) (push) Has been cancelled
SITL Tests / Testing PX4 tailsitter (push) Has been cancelled
SITL Tests / Testing PX4 iris (push) Has been cancelled
SITL Tests / Testing PX4 standard_vtol (push) Has been cancelled
Handle stale issues and PRs / stale (push) Has been cancelled
Fuzzing / Fuzzing (push) Has been cancelled
Build all targets / Scan for Board Targets (push) Has been cancelled
Build all targets / Build Group [${{ matrix.group }}][${{ matrix.arch == 'nuttx' && 'x86' || 'arm64' }}] (push) Has been cancelled
Build all targets / Upload Artifacts to S3 (push) Has been cancelled
Build all targets / Create Release and Upload Artifacts (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_fmu-v5_default) (push) Has been cancelled
Checks / build (NO_NINJA_BUILD=1 px4_sitl_default) (push) Has been cancelled
Checks / build (check_format) (push) Has been cancelled
Checks / build (check_newlines) (push) Has been cancelled
Checks / build (module_documentation) (push) Has been cancelled
Checks / build (px4_fmu-v2_default stack_check) (push) Has been cancelled
Checks / build (px4_sitl_allyes) (push) Has been cancelled
Checks / build (shellcheck_all) (push) Has been cancelled
Checks / build (tests) (push) Has been cancelled
Checks / build (tests_coverage) (push) Has been cancelled
Checks / build (validate_module_configs) (push) Has been cancelled
Clang Tidy / build (push) Has been cancelled
MacOS build / build (px4_fmu-v5_default) (push) Has been cancelled
MacOS build / build (px4_sitl) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:22.04) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:24.04) (push) Has been cancelled
Container build / Set Tags and Variables (push) Has been cancelled
Container build / Build Container (amd64) (push) Has been cancelled
Container build / Build Container (arm64) (push) Has been cancelled
Container build / Deploy To Registry (push) Has been cancelled
EKF Update Change Indicator / unit_tests (push) Has been cancelled
Failsafe Simulator Build / build (failsafe_web) (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v5x (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v6x (push) Has been cancelled
FLASH usage analysis / Publish Results (push) Has been cancelled
ITCM check / Checking nxp_tropic-community (push) Has been cancelled
ITCM check / Checking px4_fmu-v5x (push) Has been cancelled
ITCM check / Checking px4_fmu-v6xrt (push) Has been cancelled
MAVROS Mission Tests / build (map[mission:MC_mission_box vehicle:iris]) (push) Has been cancelled
MAVROS Offboard Tests / build (map[test_file:mavros_posix_tests_offboard_posctl.test vehicle:iris]) (push) Has been cancelled
Nuttx Target with extra env config / build (px4_fmu-v5_default) (push) Has been cancelled
Python CI Checks / build (push) Has been cancelled
ROS Integration Tests / build (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:humble ubuntu:jammy]) (push) Has been cancelled
ROS Translation Node Tests / Build and test (map[ros_version:jazzy ubuntu:noble]) (push) Has been cancelled
SITL Tests / Testing PX4 tailsitter (push) Has been cancelled
SITL Tests / Testing PX4 iris (push) Has been cancelled
SITL Tests / Testing PX4 standard_vtol (push) Has been cancelled
Handle stale issues and PRs / stale (push) Has been cancelled
Fuzzing / Fuzzing (push) Has been cancelled
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
committed by
Matthias Grob
parent
e15752099f
commit
16f97635ce
@@ -37,7 +37,6 @@ using namespace time_literals;
|
|||||||
|
|
||||||
void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporter)
|
void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporter)
|
||||||
{
|
{
|
||||||
// RC
|
|
||||||
manual_control_setpoint_s manual_control_setpoint;
|
manual_control_setpoint_s manual_control_setpoint;
|
||||||
|
|
||||||
if (!_manual_control_setpoint_sub.copy(&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;
|
reporter.failsafeFlags().manual_control_signal_lost = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Check if RC is valid
|
// Check if manual control is valid
|
||||||
if (!manual_control_setpoint.valid
|
if (!manual_control_setpoint.valid
|
||||||
|| hrt_elapsed_time(&manual_control_setpoint.timestamp) > _param_com_rc_loss_t.get() * 1_s) {
|
|| 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) {
|
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
|
bool gcs_connection_required = _param_nav_dll_act.get() > 0
|
||||||
/*|| (rc_is_optional && reporter.failsafeFlags().manual_control_signal_lost) */;
|
/*|| (rc_is_optional && reporter.failsafeFlags().manual_control_signal_lost) */;
|
||||||
NavModes affected_modes = gcs_connection_required ? NavModes::All : NavModes::None;
|
NavModes affected_modes = gcs_connection_required ? NavModes::All : NavModes::None;
|
||||||
events::LogLevel log_level = gcs_connection_required ? events::Log::Error : events::Log::Info;
|
events::LogLevel log_level = gcs_connection_required ? events::Log::Error : events::Log::Info;
|
||||||
/* EVENT
|
/* EVENT
|
||||||
* @description
|
* @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">
|
* <profile name="dev">
|
||||||
* This check can be configured via <param>NAV_DLL_ACT</param> parameter.
|
* This check can be configured via <param>NAV_DLL_ACT</param> parameter.
|
||||||
|
|||||||
@@ -190,7 +190,7 @@ PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0);
|
|||||||
PARAM_DEFINE_INT32(COM_RC_IN_MODE, 3);
|
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.
|
* 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);
|
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
|
* immediately gives control back to the pilot by switching to Position mode and
|
||||||
* if position is unavailable Altitude mode.
|
* if position is unavailable Altitude mode.
|
||||||
* Note: Only has an effect on multicopters, and VTOLs in multicopter 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);
|
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
|
* If COM_RC_OVERRIDE is enabled and the joystick input is moved more than this threshold
|
||||||
* the autopilot the pilot takes over control.
|
* 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);
|
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,
|
* The manual control loss failsafe will only be entered after a timeout,
|
||||||
* set by COM_RC_LOSS_T in seconds. If RC input checks have been disabled
|
* set by COM_RC_LOSS_T in seconds.
|
||||||
* by setting the COM_RC_IN_MODE param it will not be triggered.
|
|
||||||
*
|
*
|
||||||
* @value 1 Hold mode
|
* @value 1 Hold mode
|
||||||
* @value 2 Return mode
|
* @value 2 Return mode
|
||||||
@@ -620,7 +619,7 @@ PARAM_DEFINE_INT32(NAV_DLL_ACT, 0);
|
|||||||
PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
|
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.
|
* Specify modes where manual control loss is ignored and no failsafe is triggered.
|
||||||
* External modes requiring stick input will still failsafe.
|
* External modes requiring stick input will still failsafe.
|
||||||
|
|||||||
@@ -456,7 +456,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
|
|||||||
vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
|
vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
|
||||||
&& in_forward_flight && !state.mission_finished;
|
&& in_forward_flight && !state.mission_finished;
|
||||||
|
|
||||||
// Manual control (RC) loss
|
// Manual control (RC or joystick) loss
|
||||||
if (!status_flags.manual_control_signal_lost) {
|
if (!status_flags.manual_control_signal_lost) {
|
||||||
// If manual control was lost and arming was allowed, consider it optional until we regain manual control
|
// If manual control was lost and arming was allowed, consider it optional until we regain manual control
|
||||||
_manual_control_lost_at_arming = false;
|
_manual_control_lost_at_arming = false;
|
||||||
|
|||||||
@@ -109,7 +109,7 @@ TEST_F(FailsafeTest, general)
|
|||||||
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
|
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
|
||||||
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
|
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
|
||||||
|
|
||||||
// RC lost -> Hold, then RTL
|
// manual control lost -> Hold, then RTL
|
||||||
time += 10_ms;
|
time += 10_ms;
|
||||||
failsafe_flags.manual_control_signal_lost = true;
|
failsafe_flags.manual_control_signal_lost = true;
|
||||||
updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
|
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(updated_user_intented_mode, state.user_intended_mode);
|
||||||
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Descend);
|
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;
|
time += 10_ms;
|
||||||
failsafe_flags.gcs_connection_lost = false;
|
failsafe_flags.gcs_connection_lost = false;
|
||||||
updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
|
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(updated_user_intented_mode, state.user_intended_mode);
|
||||||
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
|
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
|
||||||
|
|
||||||
// RC lost cleared -> keep RTL
|
// Manual control lost cleared -> keep RTL
|
||||||
time += 10_ms;
|
time += 10_ms;
|
||||||
failsafe_flags.manual_control_signal_lost = false;
|
failsafe_flags.manual_control_signal_lost = false;
|
||||||
updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
|
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(updated_user_intented_mode, state.user_intended_mode);
|
||||||
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
|
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;
|
failsafe_flags.manual_control_signal_lost = true;
|
||||||
|
|
||||||
updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
|
updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
|
||||||
|
|||||||
@@ -289,7 +289,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
|
|||||||
_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
|
_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
|
||||||
|
|
||||||
} else {
|
} 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))) {
|
if (!PX4_ISFINITE(_land_position(0))) {
|
||||||
_land_position.xy() = Vector2f(_position);
|
_land_position.xy() = Vector2f(_position);
|
||||||
}
|
}
|
||||||
|
|||||||
+5
-5
@@ -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
|
// 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) {
|
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 height_change_speed = FOLLOW_HEIGHT_USER_ADJUST_SPEED * sticks.getThrottleZeroCenteredExpo();
|
||||||
const float new_height = _follow_height + height_change_speed * _deltatime;
|
const float new_height = _follow_height + height_change_speed * _deltatime;
|
||||||
_follow_height = constrain(new_height, MINIMUM_SAFETY_ALTITUDE, FOLLOW_HEIGHT_MAX);
|
_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
|
// 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 *
|
if (fabsf(drone_to_target_vector.length() - _follow_distance) < FOLLOW_DISTANCE_USER_ADJUST_SPEED *
|
||||||
USER_ADJUSTMENT_ERROR_TIME_WINDOW) {
|
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 distance_change_speed = FOLLOW_DISTANCE_USER_ADJUST_SPEED * sticks.getPitchExpo();
|
||||||
const float new_distance = _follow_distance + distance_change_speed * _deltatime;
|
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);
|
_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
|
// 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 *
|
if (fabsf(matrix::wrap_pi(measured_orbit_angle - tracked_orbit_angle_setpoint)) < FOLLOW_ANGLE_USER_ADJUST_SPEED *
|
||||||
USER_ADJUSTMENT_ERROR_TIME_WINDOW) {
|
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
|
// 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,
|
const float angle_adjust_speed_max = min(FOLLOW_ANGLE_USER_ADJUST_SPEED,
|
||||||
_param_flw_tgt_max_vel.get() / _follow_distance);
|
_param_flw_tgt_max_vel.get() / _follow_distance);
|
||||||
const float angle_change_speed = angle_adjust_speed_max * sticks.getRollExpo();
|
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.
|
// 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);
|
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();
|
_sticks.checkAndUpdateStickInputs();
|
||||||
|
|
||||||
if (_sticks.isAvailable()) {
|
if (_sticks.isAvailable()) {
|
||||||
|
|||||||
+17
-17
@@ -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
|
// [m/s^2] Maximum acceleration setting for generating the Follow Target Orbit trajectory
|
||||||
static constexpr float ORBIT_TRAJECTORY_MAX_ACCELERATION = 2.0;
|
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;
|
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;
|
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;
|
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;
|
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;
|
static constexpr float FOLLOW_ANGLE_USER_ADJUST_SPEED = 1.5;
|
||||||
|
|
||||||
// [s] Time window constant that gets multiplied to user adjustment speed, to calculate the
|
// [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
|
// '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
|
// Prevents setpoints diverging from the vehicle's actual position too much
|
||||||
static constexpr float USER_ADJUSTMENT_ERROR_TIME_WINDOW = 0.5f;
|
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
|
* 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);
|
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
|
* 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
|
* @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);
|
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
|
* 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 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
|
* @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
|
// Second Order Filter to calculate kinematically feasible target position
|
||||||
SecondOrderReferenceModel<matrix::Vector3f> _target_position_velocity_filter;
|
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_distance{8.0f}; // [m]
|
||||||
float _follow_height{10.0f}; // [m]
|
float _follow_height{10.0f}; // [m]
|
||||||
float _follow_angle_rad{0.0f}; // [rad]
|
float _follow_angle_rad{0.0f}; // [rad]
|
||||||
|
|||||||
@@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(MC_ORBIT_RAD_MAX, 1000.0f);
|
|||||||
* @value 1 Hold Initial Heading
|
* @value 1 Hold Initial Heading
|
||||||
* @value 2 Uncontrolled
|
* @value 2 Uncontrolled
|
||||||
* @value 3 Hold Front Tangent to Circle
|
* @value 3 Hold Front Tangent to Circle
|
||||||
* @value 4 RC Controlled
|
* @value 4 Manually (yaw stick) Controlled
|
||||||
* @group Flight Task Orbit
|
* @group Flight Task Orbit
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_INT32(MC_ORBIT_YAW_MOD, 0);
|
PARAM_DEFINE_INT32(MC_ORBIT_YAW_MOD, 0);
|
||||||
|
|||||||
@@ -105,9 +105,9 @@ PARAM_DEFINE_INT32(FW_AT_APPLY, 2);
|
|||||||
PARAM_DEFINE_INT32(FW_AT_AXES, 3);
|
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 0 Disable
|
||||||
* @value 1 Aux1
|
* @value 1 Aux1
|
||||||
|
|||||||
Reference in New Issue
Block a user