diff --git a/src/lib/rover_control/rovercontrol_params.c b/src/lib/rover_control/rovercontrol_params.c index 5a34b76433..3683384f5a 100644 --- a/src/lib/rover_control/rovercontrol_params.c +++ b/src/lib/rover_control/rovercontrol_params.c @@ -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 * diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp index a750d622eb..38be9532c5 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp @@ -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 - (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 + (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(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(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(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(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 diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp index f3c27575d2..3d5eb1974a 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp @@ -126,6 +126,8 @@ private: (ParamFloat) _param_ro_yaw_rate_limit, (ParamFloat) _param_ro_yaw_p, (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_ro_yaw_expo, + (ParamFloat) _param_ro_yaw_supexpo, (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_ro_speed_limit ) diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp index dbb0645a6e..0b116708a9 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp @@ -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 + (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(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(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(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(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(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(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 diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp index bce5d36ea5..1642afe1d7 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp @@ -124,6 +124,9 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_ro_yaw_rate_limit, (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_ro_yaw_expo, + (ParamFloat) _param_ro_yaw_supexpo, + (ParamFloat) _param_rd_yaw_stk_gain, (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_ro_speed_limit ) diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index 4a1132b857..b448753bf1 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -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 diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp index 9984fd10cd..aba3d23f9f 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp @@ -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 + (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 (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(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(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(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(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(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 diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp index 52bbb27fb3..1c03956be4 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp @@ -126,6 +126,9 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_ro_yaw_rate_limit, (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_ro_yaw_expo, + (ParamFloat) _param_ro_yaw_supexpo, + (ParamFloat) _param_rm_yaw_stk_gain, (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_ro_speed_limit, (ParamFloat) _param_rm_course_ctl_th diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index 778f548ac2..1ff9558f73 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -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