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

rover: add exponential scaling to manual yaw rate input

Browse Source
This commit is contained in:
chfriedrich98
2025-08-14 12:00:33 +02:00
committed by chfriedrich98
parent 8eb1c6063e
commit e06f7f243c
9 changed files with 86 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/rover_control/rovercontrol_params.c
+31
View File
@@ -50,6 +50,37 @@
*/
PARAM_DEFINE_FLOAT(RO_YAW_STICK_DZ, 0.1f);
/**
* Yaw rate expo factor
*
* Exponential factor for tuning the input curve shape.
*
* 0 Purely linear input curve
* 1 Purely cubic input curve
*
* @min 0
* @max 1
* @decimal 2
* @group Rover Rate Control
*/
PARAM_DEFINE_FLOAT(RO_YAW_EXPO, 0.f);
/**
* Yaw rate super expo factor
*
* "Superexponential" factor for refining the input curve shape tuned using RO_YAW_EXPO.
*
* 0 Pure Expo function
* 0.7 reasonable shape enhancement for intuitive stick feel
* 0.95 very strong bent input curve only near maxima have effect
*
* @min 0
* @max 0.95
* @decimal 2
* @group Rover Rate Control
*/
PARAM_DEFINE_FLOAT(RO_YAW_SUPEXPO, 0.f);
/**
* Yaw rate measurement threshold
*
src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp
+9 -6
View File
@@ -78,8 +78,9 @@ void AckermannManualMode::acro()
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = hrt_absolute_time();
rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * math::interpolate<float>
(manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate *
math::superexpo<float>
(manual_control_setpoint.roll, _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get());
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
}
@@ -107,8 +108,9 @@ void AckermannManualMode::stab()
// Rate control
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = hrt_absolute_time();
rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);;
rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate *
math::superexpo<float>(math::deadzone(manual_control_setpoint.roll,
_param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get());
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
// Set uncontrolled setpoint invalid
@@ -172,8 +174,9 @@ void AckermannManualMode::position()
// Rate control
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = hrt_absolute_time();
rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);;
rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(manual_control_setpoint.throttle) * _max_yaw_rate *
math::superexpo<float>(math::deadzone(manual_control_setpoint.roll,
_param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get());
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
// Set uncontrolled setpoints invalid
src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp
+2
View File
@@ -126,6 +126,8 @@ private:
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz,
(ParamFloat<px4::params::RO_YAW_EXPO>) _param_ro_yaw_expo,
(ParamFloat<px4::params::RO_YAW_SUPEXPO>) _param_ro_yaw_supexpo,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit
)
src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp
+8 -7
View File
@@ -58,7 +58,8 @@ void DifferentialManualMode::manual()
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = hrt_absolute_time();
rover_steering_setpoint.normalized_steering_setpoint = manual_control_setpoint.roll;
rover_steering_setpoint.normalized_steering_setpoint = _param_rd_yaw_stk_gain.get() * math::superexpo<float>
(manual_control_setpoint.roll, _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get());
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = hrt_absolute_time();
@@ -78,8 +79,8 @@ void DifferentialManualMode::acro()
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = hrt_absolute_time();
rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.roll, -1.f, 1.f,
-_max_yaw_rate, _max_yaw_rate);
rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo<float>(manual_control_setpoint.roll,
_param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get());
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
}
@@ -107,8 +108,8 @@ void DifferentialManualMode::stab()
// Rate control
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = hrt_absolute_time();
rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);;
rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo<float>(math::deadzone(
manual_control_setpoint.roll, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get());
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
// Set uncontrolled setpoint invalid
@@ -165,8 +166,8 @@ void DifferentialManualMode::position()
// Rate control
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = hrt_absolute_time();
rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);;
rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo<float>(math::deadzone(
manual_control_setpoint.roll, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get());
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
// Set uncontrolled setpoints invalid
src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp
+3
View File
@@ -124,6 +124,9 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz,
(ParamFloat<px4::params::RO_YAW_EXPO>) _param_ro_yaw_expo,
(ParamFloat<px4::params::RO_YAW_SUPEXPO>) _param_ro_yaw_supexpo,
(ParamFloat<px4::params::RD_YAW_STK_GAIN>) _param_rd_yaw_stk_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit
)
src/modules/rover_differential/module.yaml
+11
View File
@@ -44,3 +44,14 @@ parameters:
increment: 0.01
decimal: 3
default: 0.174533
RD_YAW_STK_GAIN:
description:
short: Yaw stick gain for Manual mode
long: Assign value <1.0 to decrease stick response for yaw control.
type: float
min: 0.1
max: 1
increment: 0.01
decimal: 3
default: 1
src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp
+8 -7
View File
@@ -58,7 +58,8 @@ void MecanumManualMode::manual()
_manual_control_setpoint_sub.copy(&manual_control_setpoint);
rover_steering_setpoint_s rover_steering_setpoint{};
rover_steering_setpoint.timestamp = hrt_absolute_time();
rover_steering_setpoint.normalized_steering_setpoint = manual_control_setpoint.yaw;
rover_steering_setpoint.normalized_steering_setpoint = _param_rm_yaw_stk_gain.get() * math::superexpo<float>
(manual_control_setpoint.yaw, _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get());
_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = hrt_absolute_time();
@@ -78,8 +79,8 @@ void MecanumManualMode::acro()
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = hrt_absolute_time();
rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float> (manual_control_setpoint.yaw, -1.f, 1.f,
-_max_yaw_rate, _max_yaw_rate);
rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo<float>(manual_control_setpoint.yaw,
_param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get());
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
}
@@ -106,8 +107,8 @@ void MecanumManualMode::stab()
// Rate control
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = hrt_absolute_time();
rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo<float>(math::deadzone(
manual_control_setpoint.yaw, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get());
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
// Set uncontrolled setpoint invalid
@@ -169,8 +170,8 @@ void MecanumManualMode::position()
// Rate control
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = hrt_absolute_time();
rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);;
rover_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo<float>(math::deadzone(
manual_control_setpoint.yaw, _param_ro_yaw_stick_dz.get()), _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get());
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
// Set uncontrolled setpoints invalid
src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp
+3
View File
@@ -126,6 +126,9 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz,
(ParamFloat<px4::params::RO_YAW_EXPO>) _param_ro_yaw_expo,
(ParamFloat<px4::params::RO_YAW_SUPEXPO>) _param_ro_yaw_supexpo,
(ParamFloat<px4::params::RM_YAW_STK_GAIN>) _param_rm_yaw_stk_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RM_COURSE_CTL_TH>) _param_rm_course_ctl_th
src/modules/rover_mecanum/module.yaml
+11
View File
@@ -30,3 +30,14 @@ parameters:
increment: 0.01
decimal: 2
default: 0.17
RM_YAW_STK_GAIN:
description:
short: Yaw stick gain for Manual mode
long: Assign value <1.0 to decrease stick response for yaw control.
type: float
min: 0.1
max: 1
increment: 0.01
decimal: 3
default: 1