From 9c38602c1254f383780bebc42e231774777039b5 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Fri, 5 Sep 2025 13:48:17 +0200 Subject: [PATCH] sv: rename rover parameters to surface vehicle --- src/lib/rtl/rtl_time_estimator.cpp | 2 +- ...trol_params.c => surface_vehicle_params.c} | 100 +++++++++--------- .../AckermannActControl.cpp | 8 +- .../AckermannActControl.hpp | 6 +- .../AckermannAttControl.cpp | 12 +-- .../AckermannAttControl.hpp | 10 +- .../AckermannAutoMode/AckermannAutoMode.cpp | 10 +- .../AckermannAutoMode/AckermannAutoMode.hpp | 8 +- .../AckermannManualMode.cpp | 10 +- .../AckermannManualMode.hpp | 12 +-- .../AckermannPosControl.cpp | 20 ++-- .../AckermannPosControl.hpp | 16 +-- .../AckermannRateControl.cpp | 24 ++--- .../AckermannRateControl.hpp | 18 ++-- .../AckermannSpeedControl.cpp | 22 ++-- .../AckermannSpeedControl.hpp | 16 +-- .../DifferentialActControl.cpp | 8 +- .../DifferentialActControl.hpp | 6 +- .../DifferentialAttControl.cpp | 12 +-- .../DifferentialAttControl.hpp | 6 +- .../DifferentialAutoMode.cpp | 4 +- .../DifferentialAutoMode.hpp | 4 +- .../DifferentialManualMode.cpp | 12 +-- .../DifferentialManualMode.hpp | 10 +- .../DifferentialPosControl.cpp | 18 ++-- .../DifferentialPosControl.hpp | 14 +-- .../DifferentialRateControl.cpp | 18 ++-- .../DifferentialRateControl.hpp | 14 +-- .../DifferentialSpeedControl.cpp | 28 ++--- .../DifferentialSpeedControl.hpp | 16 +-- .../MecanumActControl/MecanumActControl.cpp | 14 +-- .../MecanumActControl/MecanumActControl.hpp | 6 +- .../MecanumAttControl/MecanumAttControl.cpp | 12 +-- .../MecanumAttControl/MecanumAttControl.hpp | 6 +- .../MecanumAutoMode/MecanumAutoMode.cpp | 4 +- .../MecanumAutoMode/MecanumAutoMode.hpp | 4 +- .../MecanumManualMode/MecanumManualMode.cpp | 14 +-- .../MecanumManualMode/MecanumManualMode.hpp | 10 +- .../MecanumPosControl/MecanumPosControl.cpp | 16 +-- .../MecanumPosControl/MecanumPosControl.hpp | 24 ++--- .../MecanumRateControl/MecanumRateControl.cpp | 24 ++--- .../MecanumRateControl/MecanumRateControl.hpp | 16 +-- .../MecanumSpeedControl.cpp | 46 ++++---- .../MecanumSpeedControl.hpp | 16 +-- 44 files changed, 338 insertions(+), 338 deletions(-) rename src/lib/surface_vehicle_control/{rovercontrol_params.c => surface_vehicle_params.c} (73%) diff --git a/src/lib/rtl/rtl_time_estimator.cpp b/src/lib/rtl/rtl_time_estimator.cpp index ddc851c5e3..c448f77d99 100644 --- a/src/lib/rtl/rtl_time_estimator.cpp +++ b/src/lib/rtl/rtl_time_estimator.cpp @@ -55,7 +55,7 @@ RtlTimeEstimator::RtlTimeEstimator() : ModuleParams(nullptr) _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); _param_mpc_xy_cruise = param_find("MPC_XY_CRUISE"); - _param_rover_cruise_speed = param_find("RO_SPEED_LIM"); + _param_rover_cruise_speed = param_find("SV_SPEED_LIM"); }; rtl_time_estimate_s RtlTimeEstimator::getEstimate() const diff --git a/src/lib/surface_vehicle_control/rovercontrol_params.c b/src/lib/surface_vehicle_control/surface_vehicle_params.c similarity index 73% rename from src/lib/surface_vehicle_control/rovercontrol_params.c rename to src/lib/surface_vehicle_control/surface_vehicle_params.c index 3683384f5a..2cdbf0a99c 100644 --- a/src/lib/surface_vehicle_control/rovercontrol_params.c +++ b/src/lib/surface_vehicle_control/surface_vehicle_params.c @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file rovercontrol_params.c + * @file surface_vehicle_params.c * - * Parameters defined by the rover control lib. + * Parameters defined by the surface vehicle control lib. */ /** @@ -46,9 +46,9 @@ * @max 1 * @increment 0.01 * @decimal 2 - * @group Rover Rate Control + * @group SV Rate Control */ -PARAM_DEFINE_FLOAT(RO_YAW_STICK_DZ, 0.1f); +PARAM_DEFINE_FLOAT(SV_YAW_STICK_DZ, 0.1f); /** * Yaw rate expo factor @@ -61,14 +61,14 @@ PARAM_DEFINE_FLOAT(RO_YAW_STICK_DZ, 0.1f); * @min 0 * @max 1 * @decimal 2 - * @group Rover Rate Control + * @group SV Rate Control */ -PARAM_DEFINE_FLOAT(RO_YAW_EXPO, 0.f); +PARAM_DEFINE_FLOAT(SV_YAW_EXPO, 0.f); /** * Yaw rate super expo factor * - * "Superexponential" factor for refining the input curve shape tuned using RO_YAW_EXPO. + * "Superexponential" factor for refining the input curve shape tuned using SV_YAW_EXPO. * * 0 Pure Expo function * 0.7 reasonable shape enhancement for intuitive stick feel @@ -77,9 +77,9 @@ PARAM_DEFINE_FLOAT(RO_YAW_EXPO, 0.f); * @min 0 * @max 0.95 * @decimal 2 - * @group Rover Rate Control + * @group SV Rate Control */ -PARAM_DEFINE_FLOAT(RO_YAW_SUPEXPO, 0.f); +PARAM_DEFINE_FLOAT(SV_YAW_SUPEXPO, 0.f); /** * Yaw rate measurement threshold @@ -91,9 +91,9 @@ PARAM_DEFINE_FLOAT(RO_YAW_SUPEXPO, 0.f); * @max 100 * @increment 0.01 * @decimal 2 - * @group Rover Rate Control + * @group SV Rate Control */ -PARAM_DEFINE_FLOAT(RO_YAW_RATE_TH, 3.f); +PARAM_DEFINE_FLOAT(SV_YAW_RATE_TH, 3.f); /** * Proportional gain for closed loop yaw rate controller @@ -102,9 +102,9 @@ PARAM_DEFINE_FLOAT(RO_YAW_RATE_TH, 3.f); * @max 100 * @increment 0.01 * @decimal 3 - * @group Rover Rate Control + * @group SV Rate Control */ -PARAM_DEFINE_FLOAT(RO_YAW_RATE_P, 0.f); +PARAM_DEFINE_FLOAT(SV_YAW_RATE_P, 0.f); /** * Integral gain for closed loop yaw rate controller @@ -113,9 +113,9 @@ PARAM_DEFINE_FLOAT(RO_YAW_RATE_P, 0.f); * @max 100 * @increment 0.01 * @decimal 3 - * @group Rover Rate Control + * @group SV Rate Control */ -PARAM_DEFINE_FLOAT(RO_YAW_RATE_I, 0.f); +PARAM_DEFINE_FLOAT(SV_YAW_RATE_I, 0.f); /** * Yaw rate limit @@ -128,9 +128,9 @@ PARAM_DEFINE_FLOAT(RO_YAW_RATE_I, 0.f); * @max 10000 * @increment 0.01 * @decimal 2 - * @group Rover Rate Control + * @group SV Rate Control */ -PARAM_DEFINE_FLOAT(RO_YAW_RATE_LIM, 0.f); +PARAM_DEFINE_FLOAT(SV_YAW_RATE_LIM, 0.f); /** * Yaw acceleration limit @@ -143,9 +143,9 @@ PARAM_DEFINE_FLOAT(RO_YAW_RATE_LIM, 0.f); * @max 10000 * @increment 0.01 * @decimal 2 - * @group Rover Rate Control + * @group SV Rate Control */ -PARAM_DEFINE_FLOAT(RO_YAW_ACCEL_LIM, -1.f); +PARAM_DEFINE_FLOAT(SV_YAW_ACCEL_LIM, -1.f); /** * Yaw deceleration limit @@ -158,9 +158,9 @@ PARAM_DEFINE_FLOAT(RO_YAW_ACCEL_LIM, -1.f); * @max 10000 * @increment 0.01 * @decimal 2 - * @group Rover Rate Control + * @group SV Rate Control */ -PARAM_DEFINE_FLOAT(RO_YAW_DECEL_LIM, -1.f); +PARAM_DEFINE_FLOAT(SV_YAW_DECEL_LIM, -1.f); /** * Yaw rate correction factor @@ -174,9 +174,9 @@ PARAM_DEFINE_FLOAT(RO_YAW_DECEL_LIM, -1.f); * @max 10000 * @increment 0.01 * @decimal 2 - * @group Rover Rate Control + * @group SV Rate Control */ -PARAM_DEFINE_FLOAT(RO_YAW_RATE_CORR, 1.f); +PARAM_DEFINE_FLOAT(SV_YAW_RATE_CORR, 1.f); /** * Proportional gain for closed loop yaw controller @@ -185,12 +185,12 @@ PARAM_DEFINE_FLOAT(RO_YAW_RATE_CORR, 1.f); * @max 100 * @increment 0.01 * @decimal 3 - * @group Rover Attitude Control + * @group SV Attitude Control */ -PARAM_DEFINE_FLOAT(RO_YAW_P, 0.f); +PARAM_DEFINE_FLOAT(SV_YAW_P, 0.f); /** - * Speed the rover drives at maximum throttle + * Speed the vehicle drives at maximum throttle * * Used to linearly map speeds [m/s] to throttle values [-1. 1]. * @@ -199,9 +199,9 @@ PARAM_DEFINE_FLOAT(RO_YAW_P, 0.f); * @unit m/s * @increment 0.01 * @decimal 2 - * @group Rover Velocity Control + * @group SV Speed Control */ -PARAM_DEFINE_FLOAT(RO_MAX_THR_SPEED, 0.f); +PARAM_DEFINE_FLOAT(SV_MAX_THR_SPEED, 0.f); /** * Proportional gain for ground speed controller @@ -210,9 +210,9 @@ PARAM_DEFINE_FLOAT(RO_MAX_THR_SPEED, 0.f); * @max 100 * @increment 0.01 * @decimal 2 - * @group Rover Velocity Control + * @group SV Speed Control */ -PARAM_DEFINE_FLOAT(RO_SPEED_P, 0.f); +PARAM_DEFINE_FLOAT(SV_SPEED_P, 0.f); /** * Integral gain for ground speed controller @@ -221,9 +221,9 @@ PARAM_DEFINE_FLOAT(RO_SPEED_P, 0.f); * @max 100 * @increment 0.001 * @decimal 3 - * @group Rover Velocity Control + * @group SV Speed Control */ -PARAM_DEFINE_FLOAT(RO_SPEED_I, 0.f); +PARAM_DEFINE_FLOAT(SV_SPEED_I, 0.f); /** * Speed limit @@ -235,56 +235,56 @@ PARAM_DEFINE_FLOAT(RO_SPEED_I, 0.f); * @max 100 * @increment 0.01 * @decimal 2 - * @group Rover Velocity Control + * @group SV Speed Control */ -PARAM_DEFINE_FLOAT(RO_SPEED_LIM, -1.f); +PARAM_DEFINE_FLOAT(SV_SPEED_LIM, -1.f); /** * Acceleration limit * * Set to -1 to disable. - * For mecanum rovers this limit is used for longitudinal and lateral acceleration. + * For omnidirectional vehicles this limit is used for longitudinal and lateral acceleration. * * @unit m/s^2 * @min -1 * @max 100 * @increment 0.01 * @decimal 2 - * @group Rover Velocity Control + * @group SV Speed Control */ -PARAM_DEFINE_FLOAT(RO_ACCEL_LIM, -1.f); +PARAM_DEFINE_FLOAT(SV_ACCEL_LIM, -1.f); /** * Deceleration limit * * Set to -1 to disable. - * Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. - * For mecanum rovers this limit is used for longitudinal and lateral deceleration. + * Note that if it is disabled the vehicle will not slow down when approaching waypoints in auto modes. + * For omnidirectional vehicles this limit is used for longitudinal and lateral deceleration. * * @unit m/s^2 * @min -1 * @max 100 * @increment 0.01 * @decimal 2 - * @group Rover Velocity Control + * @group SV Speed Control */ -PARAM_DEFINE_FLOAT(RO_DECEL_LIM, -1.f); +PARAM_DEFINE_FLOAT(SV_DECEL_LIM, -1.f); /** * Jerk limit * * Set to -1 to disable. - * Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. - * For mecanum rovers this limit is used for longitudinal and lateral jerk. + * Note that if it is disabled the vehicle will not slow down when approaching waypoints in auto modes. + * For omnidirectional vehicles this limit is used for longitudinal and lateral jerk. * * @unit m/s^3 * @min -1 * @max 100 * @increment 0.01 * @decimal 2 - * @group Rover Velocity Control + * @group SV Speed Control */ -PARAM_DEFINE_FLOAT(RO_JERK_LIM, -1.f); +PARAM_DEFINE_FLOAT(SV_JERK_LIM, -1.f); /** * Speed measurement threshold @@ -297,14 +297,14 @@ PARAM_DEFINE_FLOAT(RO_JERK_LIM, -1.f); * @max 100 * @increment 0.01 * @decimal 2 - * @group Rover Velocity Control + * @group SV Speed Control */ -PARAM_DEFINE_FLOAT(RO_SPEED_TH, 0.1f); +PARAM_DEFINE_FLOAT(SV_SPEED_TH, 0.1f); /** * Tuning parameter for the speed reduction based on the course error * - * Reduced_speed = RO_MAX_THR_SPEED * (1 - normalized_course_error * RO_SPEED_RED) + * Reduced_speed = SV_MAX_THR_SPEED * (1 - normalized_course_error * SV_SPEED_RED) * The normalized course error is the angle between the current course and the bearing setpoint * interpolated from [0, 180] -> [0, 1]. * Higher value -> More speed reduction. @@ -315,6 +315,6 @@ PARAM_DEFINE_FLOAT(RO_SPEED_TH, 0.1f); * @max 100 * @increment 0.01 * @decimal 2 - * @group Rover Velocity Control + * @group SV Speed Control */ -PARAM_DEFINE_FLOAT(RO_SPEED_RED, -1.f); +PARAM_DEFINE_FLOAT(SV_SPEED_RED, -1.f); diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp index 8199ea73fe..b3d7feaf53 100644 --- a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp +++ b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp @@ -48,8 +48,8 @@ void AckermannActControl::updateParams() _servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_ra_str_rate_limit.get()) / _param_ra_max_str_ang.get()); } - if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { - _motor_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + if (_param_sv_accel_limit.get() > FLT_EPSILON && _param_sv_max_thr_speed.get() > FLT_EPSILON) { + _motor_setpoint.setSlewRate(_param_sv_accel_limit.get() / _param_sv_max_thr_speed.get()); } } @@ -72,8 +72,8 @@ void AckermannActControl::updateActControl() actuator_motors_s actuator_motors{}; actuator_motors.reversible_flags = _param_r_rev.get(); actuator_motors.control[0] = SurfaceVehicleControl::throttleControl(_motor_setpoint, - _throttle_setpoint, actuator_motors_sub.control[0], _param_ro_accel_limit.get(), - _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); + _throttle_setpoint, actuator_motors_sub.control[0], _param_sv_accel_limit.get(), + _param_sv_decel_limit.get(), _param_sv_max_thr_speed.get(), dt); actuator_motors.timestamp = _timestamp; _actuator_motors_pub.publish(actuator_motors); diff --git a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp index b3629e9e8c..4e01622929 100644 --- a/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp +++ b/src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp @@ -104,8 +104,8 @@ private: (ParamInt) _param_r_rev, (ParamFloat) _param_ra_str_rate_limit, (ParamFloat) _param_ra_max_str_ang, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_max_thr_speed + (ParamFloat) _param_sv_accel_limit, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_max_thr_speed ) }; diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp index 514721fd9d..5fc4760b6f 100644 --- a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp @@ -46,12 +46,12 @@ void AckermannAttControl::updateParams() { ModuleParams::updateParams(); - if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + if (_param_sv_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; } // Set up PID controller - _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setGains(_param_sv_yaw_p.get(), 0.f, 0.f); _pid_yaw.setIntegralLimit(0.f); _pid_yaw.setOutputLimit(_max_yaw_rate); @@ -106,7 +106,7 @@ void AckermannAttControl::updateSubscriptions() actuator_motors_s actuator_motors; _actuator_motors_sub.copy(&actuator_motors); _estimated_speed_body_x = math::interpolate (actuator_motors.control[0], -1.f, 1.f, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + -_param_sv_max_thr_speed.get(), _param_sv_max_thr_speed.get()); } if (_surface_vehicle_attitude_setpoint_sub.updated()) { @@ -120,7 +120,7 @@ bool AckermannAttControl::runSanityChecks() { bool ret = true; - if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + if (_param_sv_max_thr_speed.get() < FLT_EPSILON) { ret = false; } @@ -132,7 +132,7 @@ bool AckermannAttControl::runSanityChecks() ret = false; } - if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + if (_param_sv_yaw_rate_limit.get() < FLT_EPSILON) { ret = false; } diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp index 696d41f140..f23562c48b 100644 --- a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp @@ -107,7 +107,7 @@ private: hrt_abstime _timestamp{0}; float _max_yaw_rate{0.f}; float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed_body_x] - between [0, 0] and [1, _param_ro_max_thr_speed].*/ + between [0, 0] and [1, _param_sv_max_thr_speed].*/ float _yaw_setpoint{NAN}; // Controllers @@ -116,11 +116,11 @@ private: // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_sv_max_thr_speed, (ParamFloat) _param_ra_wheel_base, (ParamFloat) _param_ra_max_str_ang, - (ParamFloat) _param_ro_yaw_rate_limit, - (ParamFloat) _param_ro_yaw_p, - (ParamFloat) _param_ro_yaw_stick_dz + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_p, + (ParamFloat) _param_sv_yaw_stick_dz ) }; diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp index 295a91c736..729d8003f0 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp @@ -44,7 +44,7 @@ AckermannAutoMode::AckermannAutoMode(ModuleParams *parent) : ModuleParams(parent void AckermannAutoMode::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) { @@ -109,7 +109,7 @@ void AckermannAutoMode::updateWaypointsAndAcceptanceRadius() // Waypoint cruising speed _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( - position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + position_setpoint_triplet.current.cruising_speed, 0.f, _param_sv_speed_limit.get()) : _param_sv_speed_limit.get(); } float AckermannAutoMode::updateAcceptanceRadius(const float waypoint_transition_angle, @@ -145,11 +145,11 @@ float AckermannAutoMode::arrivalSpeed(const float cruising_speed, const float mi || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) { return 0.f; // Stop at the waypoint - } else if (_param_ro_speed_red.get() > FLT_EPSILON) { - const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate( + } else if (_param_sv_speed_red.get() > FLT_EPSILON) { + const float speed_reduction = math::constrain(_param_sv_speed_red.get() * math::interpolate( M_PI_F - waypoint_transition_angle, 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); - return math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), min_speed, + return math::constrain(_param_sv_max_thr_speed.get() * (1.f - speed_reduction), min_speed, cruising_speed); // Slow down for cornering } diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp index 3ab7c1ee4d..80e3b0decd 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp @@ -127,14 +127,14 @@ private: int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE}; DEFINE_PARAMETERS( - (ParamFloat) _param_ro_yaw_rate_limit, - (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_sv_speed_limit, (ParamFloat) _param_ra_wheel_base, (ParamFloat) _param_ra_max_str_ang, (ParamFloat) _param_nav_acc_rad, (ParamFloat) _param_ra_acc_rad_max, (ParamFloat) _param_ra_acc_rad_gain, - (ParamFloat) _param_ro_speed_red, - (ParamFloat) _param_ro_max_thr_speed + (ParamFloat) _param_sv_speed_red, + (ParamFloat) _param_sv_max_thr_speed ) }; diff --git a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp index a9b97b0df9..a4d0ac778d 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp @@ -49,7 +49,7 @@ AckermannManualMode::AckermannManualMode(ModuleParams *parent) : ModuleParams(pa void AckermannManualMode::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; } void AckermannManualMode::manual() @@ -80,7 +80,7 @@ void AckermannManualMode::acro() surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); surface_vehicle_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()); + (manual_control_setpoint.roll, _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); } @@ -110,7 +110,7 @@ void AckermannManualMode::stab() surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); surface_vehicle_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()); + _param_sv_yaw_stick_dz.get()), _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); // Set uncontrolled setpoint invalid @@ -157,7 +157,7 @@ void AckermannManualMode::position() _manual_control_setpoint_sub.copy(&manual_control_setpoint); const float speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + -1.f, 1.f, -_param_sv_speed_limit.get(), _param_sv_speed_limit.get()); if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON || fabsf(speed_setpoint) < FLT_EPSILON) { @@ -174,7 +174,7 @@ void AckermannManualMode::position() surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); surface_vehicle_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()); + _param_sv_yaw_stick_dz.get()), _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_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 15e605ea7a..60a7aa5712 100644 --- a/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp +++ b/src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp @@ -123,12 +123,12 @@ private: float _max_yaw_rate{NAN}; DEFINE_PARAMETERS( - (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_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_p, + (ParamFloat) _param_sv_yaw_stick_dz, + (ParamFloat) _param_sv_yaw_expo, + (ParamFloat) _param_sv_yaw_supexpo, (ParamFloat) _param_pp_lookahd_max, - (ParamFloat) _param_ro_speed_limit + (ParamFloat) _param_sv_speed_limit ) }; diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp index 94e5dddf27..b35509c156 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp @@ -47,7 +47,7 @@ AckermannPosControl::AckermannPosControl(ModuleParams *parent) : ModuleParams(pa void AckermannPosControl::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; _min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get()); } @@ -67,8 +67,8 @@ void AckermannPosControl::updatePosControl() if (distance_to_target > _acceptance_radius || _arrival_speed > FLT_EPSILON) { - float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), - _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); + float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_sv_jerk_limit.get(), + _param_sv_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); speed_setpoint = math::min(speed_setpoint, _cruising_speed); pure_pursuit_status_s pure_pursuit_status{}; @@ -78,12 +78,12 @@ void AckermannPosControl::updatePosControl() _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _target_waypoint_ned, _start_ned, _curr_pos_ned, fabsf(speed_setpoint)); - if (_param_ro_speed_red.get() > FLT_EPSILON) { + if (_param_sv_speed_red.get() > FLT_EPSILON) { const float course_error = fabsf(matrix::wrap_pi(bearing_setpoint - _vehicle_yaw)); - const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(course_error, + const float speed_reduction = math::constrain(_param_sv_speed_red.get() * math::interpolate(course_error, 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); - const float max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), _min_speed, - _param_ro_max_thr_speed.get()); + const float max_speed = math::constrain(_param_sv_max_thr_speed.get() * (1.f - speed_reduction), _min_speed, + _param_sv_max_thr_speed.get()); speed_setpoint = math::constrain(speed_setpoint, -max_speed, max_speed); } @@ -144,7 +144,7 @@ void AckermannPosControl::updateSubscriptions() Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); - _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; + _vehicle_speed = velocity_2d.norm() > _param_sv_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } if (_surface_vehicle_position_setpoint_sub.updated()) { @@ -156,7 +156,7 @@ void AckermannPosControl::updateSubscriptions() surface_vehicle_position_setpoint.arrival_speed : 0.f; _cruising_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.cruising_speed) ? surface_vehicle_position_setpoint.cruising_speed : - _param_ro_speed_limit.get(); + _param_sv_speed_limit.get(); _target_waypoint_ned = Vector2f(surface_vehicle_position_setpoint.position_ned[0], surface_vehicle_position_setpoint.position_ned[1]); _stopped = false; @@ -168,7 +168,7 @@ bool AckermannPosControl::runSanityChecks() { bool ret = true; - if (_param_ro_speed_limit.get() < FLT_EPSILON) { + if (_param_sv_speed_limit.get() < FLT_EPSILON) { ret = false; } diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp index 4db169d6ad..277f6240f6 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp @@ -82,7 +82,7 @@ public: /** * @brief Reset position controller. */ - void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;}; + void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_sv_speed_limit.get(); _stopped = false;}; protected: /** @@ -124,17 +124,17 @@ private: uint8_t _updated_reset_counter{0}; /**< counter for estimator resets in xy-direction */ DEFINE_PARAMETERS( - (ParamFloat) _param_ro_max_thr_speed, - (ParamFloat) _param_ro_speed_red, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_jerk_limit, - (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_sv_max_thr_speed, + (ParamFloat) _param_sv_speed_red, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_jerk_limit, + (ParamFloat) _param_sv_speed_limit, (ParamFloat) _param_pp_lookahd_gain, (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_pp_lookahd_min, - (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_rate_limit, (ParamFloat) _param_ra_wheel_base, (ParamFloat) _param_ra_max_str_ang, - (ParamFloat) _param_ro_speed_th + (ParamFloat) _param_sv_speed_th ) }; diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp index 269040c0b0..2e475988c7 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp @@ -45,15 +45,15 @@ AckermannRateControl::AckermannRateControl(ModuleParams *parent) : ModuleParams( void AckermannRateControl::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; // Set up PID controller - _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setGains(_param_sv_yaw_rate_p.get(), _param_sv_yaw_rate_i.get(), 0.f); _pid_yaw_rate.setIntegralLimit(1.f); _pid_yaw_rate.setOutputLimit(1.f); // Set up slew rate - _adjusted_yaw_rate_setpoint.setSlewRate(_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F); + _adjusted_yaw_rate_setpoint.setSlewRate(_param_sv_yaw_accel_limit.get() * M_DEG_TO_RAD_F); } void AckermannRateControl::updateRateControl() @@ -73,7 +73,7 @@ void AckermannRateControl::updateRateControl() float yaw_rate_limit = math::min(max_possible_yaw_rate, _max_yaw_rate); float constrained_yaw_rate = math::constrain(_yaw_rate_setpoint, -yaw_rate_limit, yaw_rate_limit); - if (_param_ro_yaw_accel_limit.get() > FLT_EPSILON) { // Apply slew rate if configured + if (_param_sv_yaw_accel_limit.get() > FLT_EPSILON) { // Apply slew rate if configured if (fabsf(_adjusted_yaw_rate_setpoint.getState() - _vehicle_yaw_rate) > fabsf(constrained_yaw_rate - _vehicle_yaw_rate)) { _adjusted_yaw_rate_setpoint.setForcedValue(_vehicle_yaw_rate); @@ -87,7 +87,7 @@ void AckermannRateControl::updateRateControl() // Feed forward steering_setpoint = atanf(_adjusted_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed) * - _param_ro_yaw_rate_corr.get(); + _param_sv_yaw_rate_corr.get(); // Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability) if (_estimated_speed > FLT_EPSILON) { @@ -126,7 +126,7 @@ void AckermannRateControl::updateSubscriptions() if (_vehicle_angular_velocity_sub.updated()) { vehicle_angular_velocity_s vehicle_angular_velocity{}; _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); - _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_sv_yaw_rate_th.get() * M_DEG_TO_RAD_F ? vehicle_angular_velocity.xyz[2] : 0.f; } @@ -135,8 +135,8 @@ void AckermannRateControl::updateSubscriptions() actuator_motors_s actuator_motors; _actuator_motors_sub.copy(&actuator_motors); _estimated_speed = math::interpolate(actuator_motors.control[0], -1.f, 1.f, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); - _estimated_speed = fabsf(_estimated_speed) > _param_ro_speed_th.get() ? _estimated_speed : 0.f; + -_param_sv_max_thr_speed.get(), _param_sv_max_thr_speed.get()); + _estimated_speed = fabsf(_estimated_speed) > _param_sv_speed_th.get() ? _estimated_speed : 0.f; } if (_surface_vehicle_rate_setpoint_sub.updated()) { @@ -150,10 +150,10 @@ bool AckermannRateControl::runSanityChecks() { bool ret = true; - if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + if (_param_sv_max_thr_speed.get() < FLT_EPSILON) { ret = false; events::send(events::ID("ackermann_rate_control_conf_invalid_max_thr_speed"), events::Log::Error, - "Invalid configuration of necessary parameter RO_MAX_THR_SPEED", _param_ro_max_thr_speed.get()); + "Invalid configuration of necessary parameter SV_MAX_THR_SPEED", _param_sv_max_thr_speed.get()); } @@ -171,10 +171,10 @@ bool AckermannRateControl::runSanityChecks() } - if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + if (_param_sv_yaw_rate_limit.get() < FLT_EPSILON) { ret = false; events::send(events::ID("ackermann_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, - "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); + "Invalid configuration of necessary parameter SV_YAW_RATE_LIM", _param_sv_yaw_rate_limit.get()); } diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp index e87ba1752f..e141e37274 100644 --- a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp @@ -104,7 +104,7 @@ private: // Variables float _estimated_speed{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed] - between [0, 0] and [1, _param_ro_max_thr_speed].*/ + between [0, 0] and [1, _param_sv_max_thr_speed].*/ float _max_yaw_rate{0.f}; float _vehicle_yaw_rate{0.f}; float _yaw_rate_setpoint{NAN}; @@ -115,15 +115,15 @@ private: SlewRate _adjusted_yaw_rate_setpoint{0.f}; DEFINE_PARAMETERS( - (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_sv_max_thr_speed, (ParamFloat) _param_ra_wheel_base, (ParamFloat) _param_ra_max_str_ang, - (ParamFloat) _param_ro_yaw_rate_limit, - (ParamFloat) _param_ro_yaw_rate_th, - (ParamFloat) _param_ro_yaw_rate_p, - (ParamFloat) _param_ro_yaw_rate_i, - (ParamFloat) _param_ro_yaw_accel_limit, - (ParamFloat) _param_ro_speed_th, - (ParamFloat) _param_ro_yaw_rate_corr + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_rate_th, + (ParamFloat) _param_sv_yaw_rate_p, + (ParamFloat) _param_sv_yaw_rate_i, + (ParamFloat) _param_sv_yaw_accel_limit, + (ParamFloat) _param_sv_speed_th, + (ParamFloat) _param_sv_yaw_rate_corr ) }; diff --git a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp index a523206401..000f1112f6 100644 --- a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp +++ b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp @@ -47,13 +47,13 @@ void AckermannSpeedControl::updateParams() ModuleParams::updateParams(); // Set up PID controller - _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed.setGains(_param_sv_speed_p.get(), _param_sv_speed_i.get(), 0.f); _pid_speed.setIntegralLimit(1.f); _pid_speed.setOutputLimit(1.f); // Set up slew rate - if (_param_ro_accel_limit.get() > FLT_EPSILON) { - _adjusted_speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); + if (_param_sv_accel_limit.get() > FLT_EPSILON) { + _adjusted_speed_setpoint.setSlewRate(_param_sv_accel_limit.get()); } } @@ -67,14 +67,14 @@ void AckermannSpeedControl::updateSpeedControl() // Throttle Setpoint if (PX4_ISFINITE(_speed_setpoint)) { - const float speed_setpoint = math::constrain(_speed_setpoint, -_param_ro_speed_limit.get(), - _param_ro_speed_limit.get()); + const float speed_setpoint = math::constrain(_speed_setpoint, -_param_sv_speed_limit.get(), + _param_sv_speed_limit.get()); surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; surface_vehicle_throttle_setpoint.timestamp = _timestamp; surface_vehicle_throttle_setpoint.throttle_body_x = SurfaceVehicleControl::speedControl(_adjusted_speed_setpoint, _pid_speed, - speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), - _param_ro_max_thr_speed.get(), dt); + speed_setpoint, _vehicle_speed, _param_sv_accel_limit.get(), _param_sv_decel_limit.get(), + _param_sv_max_thr_speed.get(), dt); surface_vehicle_throttle_setpoint.throttle_body_y = NAN; _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); } @@ -105,7 +105,7 @@ void AckermannSpeedControl::updateSubscriptions() Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); - _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; + _vehicle_speed = velocity_2d.norm() > _param_sv_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } if (_surface_vehicle_speed_setpoint_sub.updated()) { @@ -119,14 +119,14 @@ bool AckermannSpeedControl::runSanityChecks() { bool ret = true; - if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + if (_param_sv_max_thr_speed.get() < FLT_EPSILON) { ret = false; } - if (_param_ro_speed_limit.get() < FLT_EPSILON) { + if (_param_sv_speed_limit.get() < FLT_EPSILON) { ret = false; events::send(events::ID("ackermann_speed_control_conf_invalid_speed_lim"), events::Log::Error, - "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + "Invalid configuration of necessary parameter SV_SPEED_LIM", _param_sv_speed_limit.get()); } diff --git a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp index 7f39117ff2..4d41ad4247 100644 --- a/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp +++ b/src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp @@ -116,13 +116,13 @@ private: SlewRate _adjusted_speed_setpoint{0.f}; DEFINE_PARAMETERS( - (ParamFloat) _param_ro_max_thr_speed, - (ParamFloat) _param_ro_speed_p, - (ParamFloat) _param_ro_speed_i, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_jerk_limit, - (ParamFloat) _param_ro_speed_limit, - (ParamFloat) _param_ro_speed_th + (ParamFloat) _param_sv_max_thr_speed, + (ParamFloat) _param_sv_speed_p, + (ParamFloat) _param_sv_speed_i, + (ParamFloat) _param_sv_accel_limit, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_jerk_limit, + (ParamFloat) _param_sv_speed_limit, + (ParamFloat) _param_sv_speed_th ) }; diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp index 2f24877e3a..e24bb947bc 100644 --- a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp @@ -44,8 +44,8 @@ void DifferentialActControl::updateParams() { ModuleParams::updateParams(); - if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { - _adjusted_throttle_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + if (_param_sv_accel_limit.get() > FLT_EPSILON && _param_sv_max_thr_speed.get() > FLT_EPSILON) { + _adjusted_throttle_setpoint.setSlewRate(_param_sv_accel_limit.get() / _param_sv_max_thr_speed.get()); } } @@ -73,8 +73,8 @@ void DifferentialActControl::updateActControl() _actuator_motors_sub.copy(&actuator_motors_sub); const float current_throttle = (actuator_motors_sub.control[0] + actuator_motors_sub.control[1]) / 2.f; const float adjusted_throttle_setpoint = SurfaceVehicleControl::throttleControl(_adjusted_throttle_setpoint, - _throttle_setpoint, current_throttle, _param_ro_accel_limit.get(), - _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); + _throttle_setpoint, current_throttle, _param_sv_accel_limit.get(), + _param_sv_decel_limit.get(), _param_sv_max_thr_speed.get(), dt); actuator_motors_s actuator_motors{}; actuator_motors.reversible_flags = _param_r_rev.get(); computeInverseKinematics(adjusted_throttle_setpoint, _speed_diff_setpoint).copyTo(actuator_motors.control); diff --git a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp index dc532de523..7bb4393025 100644 --- a/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp +++ b/src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp @@ -106,8 +106,8 @@ private: // Parameters DEFINE_PARAMETERS( (ParamInt) _param_r_rev, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_max_thr_speed + (ParamFloat) _param_sv_accel_limit, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_max_thr_speed ) }; diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp index e737513e6c..0f46184346 100644 --- a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp @@ -46,12 +46,12 @@ void DifferentialAttControl::updateParams() { ModuleParams::updateParams(); - if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + if (_param_sv_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; } // Set up PID controller - _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setGains(_param_sv_yaw_p.get(), 0.f, 0.f); _pid_yaw.setIntegralLimit(_max_yaw_rate); _pid_yaw.setOutputLimit(_max_yaw_rate); @@ -101,14 +101,14 @@ bool DifferentialAttControl::runSanityChecks() { bool ret = true; - if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + if (_param_sv_yaw_rate_limit.get() < FLT_EPSILON) { ret = false; } - if (_param_ro_yaw_p.get() < FLT_EPSILON) { + if (_param_sv_yaw_p.get() < FLT_EPSILON) { ret = false; events::send(events::ID("differential_att_control_conf_invalid_yaw_p"), events::Log::Error, - "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get()); + "Invalid configuration of necessary parameter SV_YAW_P", _param_sv_yaw_p.get()); } return ret; diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp index 40dfd3a613..8d1e0be7a8 100644 --- a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp @@ -108,8 +108,8 @@ private: // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_ro_yaw_rate_limit, - (ParamFloat) _param_ro_yaw_p, - (ParamFloat) _param_ro_yaw_stick_dz + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_p, + (ParamFloat) _param_sv_yaw_stick_dz ) }; diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp index dd65a4d230..c4939494aa 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp @@ -77,7 +77,7 @@ void DifferentialAutoMode::autoControl() // Waypoint cruising speed float cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( - position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + position_setpoint_triplet.current.cruising_speed, 0.f, _param_sv_speed_limit.get()) : _param_sv_speed_limit.get(); surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); @@ -86,7 +86,7 @@ void DifferentialAutoMode::autoControl() surface_vehicle_position_setpoint.start_ned[0] = prev_wp_ned(0); surface_vehicle_position_setpoint.start_ned[1] = prev_wp_ned(1); surface_vehicle_position_setpoint.arrival_speed = arrivalSpeed(cruising_speed, waypoint_transition_angle, - _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), _param_ro_speed_red.get(), curr_wp_type); + _param_sv_speed_limit.get(), _param_rd_trans_drv_trn.get(), _param_sv_speed_red.get(), curr_wp_type); surface_vehicle_position_setpoint.cruising_speed = cruising_speed; surface_vehicle_position_setpoint.yaw = NAN; _surface_vehicle_position_setpoint_pub.publish(surface_vehicle_position_setpoint); diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp index a853c5225e..25e850cd63 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp @@ -94,8 +94,8 @@ private: uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; DEFINE_PARAMETERS( - (ParamFloat) _param_ro_speed_limit, - (ParamFloat) _param_ro_speed_red, + (ParamFloat) _param_sv_speed_limit, + (ParamFloat) _param_sv_speed_red, (ParamFloat) _param_rd_trans_drv_trn ) }; diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp index db38ddfda0..478512755c 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp @@ -49,7 +49,7 @@ DifferentialManualMode::DifferentialManualMode(ModuleParams *parent) : ModulePar void DifferentialManualMode::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; } void DifferentialManualMode::manual() @@ -59,7 +59,7 @@ void DifferentialManualMode::manual() surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; surface_vehicle_steering_setpoint.timestamp = hrt_absolute_time(); surface_vehicle_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()); + (manual_control_setpoint.roll, _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); @@ -80,7 +80,7 @@ void DifferentialManualMode::acro() surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); surface_vehicle_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(manual_control_setpoint.roll, - _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); + _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); } @@ -109,7 +109,7 @@ void DifferentialManualMode::stab() surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); surface_vehicle_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()); + manual_control_setpoint.roll, _param_sv_yaw_stick_dz.get()), _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); // Set uncontrolled setpoint invalid @@ -149,7 +149,7 @@ void DifferentialManualMode::position() _manual_control_setpoint_sub.copy(&manual_control_setpoint); const float speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + -1.f, 1.f, -_param_sv_speed_limit.get(), _param_sv_speed_limit.get()); if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON || fabsf(speed_setpoint) < FLT_EPSILON) { @@ -165,7 +165,7 @@ void DifferentialManualMode::position() surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); surface_vehicle_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()); + manual_control_setpoint.roll, _param_sv_yaw_stick_dz.get()), _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_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 85bee48ff1..777c694703 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp @@ -122,12 +122,12 @@ private: float _max_yaw_rate{NAN}; 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_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_stick_dz, + (ParamFloat) _param_sv_yaw_expo, + (ParamFloat) _param_sv_yaw_supexpo, (ParamFloat) _param_rd_yaw_stk_gain, (ParamFloat) _param_pp_lookahd_max, - (ParamFloat) _param_ro_speed_limit + (ParamFloat) _param_sv_speed_limit ) }; diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp index 5e0bfb9977..f8788b0588 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp @@ -65,8 +65,8 @@ void DifferentialPosControl::updatePosControl() } if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) { - float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), - _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); + float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_sv_jerk_limit.get(), + _param_sv_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); speed_setpoint = math::min(speed_setpoint, _cruising_speed); pure_pursuit_status_s pure_pursuit_status{}; @@ -89,11 +89,11 @@ void DifferentialPosControl::updatePosControl() if (_current_state == DrivingState::SPOT_TURNING) { speed_setpoint = 0.f; // stop during spot turning - } else if (_param_ro_speed_red.get() > FLT_EPSILON) { - const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(fabsf(heading_error), + } else if (_param_sv_speed_red.get() > FLT_EPSILON) { + const float speed_reduction = math::constrain(_param_sv_speed_red.get() * math::interpolate(fabsf(heading_error), 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); - const float max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), 0.f, - _param_ro_max_thr_speed.get()); + const float max_speed = math::constrain(_param_sv_max_thr_speed.get() * (1.f - speed_reduction), 0.f, + _param_sv_max_thr_speed.get()); speed_setpoint = math::constrain(speed_setpoint, -max_speed, max_speed); } @@ -150,7 +150,7 @@ void DifferentialPosControl::updateSubscriptions() Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); - _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; + _vehicle_speed = velocity_2d.norm() > _param_sv_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } if (_surface_vehicle_position_setpoint_sub.updated()) { @@ -162,7 +162,7 @@ void DifferentialPosControl::updateSubscriptions() surface_vehicle_position_setpoint.arrival_speed : 0.f; _cruising_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.cruising_speed) ? surface_vehicle_position_setpoint.cruising_speed : - _param_ro_speed_limit.get(); + _param_sv_speed_limit.get(); _target_waypoint_ned = Vector2f(surface_vehicle_position_setpoint.position_ned[0], surface_vehicle_position_setpoint.position_ned[1]); _stopped = false; @@ -173,7 +173,7 @@ bool DifferentialPosControl::runSanityChecks() { bool ret = true; - if (_param_ro_speed_limit.get() < FLT_EPSILON) { + if (_param_sv_speed_limit.get() < FLT_EPSILON) { ret = false; } diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp index 99cd5a6d1a..e9b1541ff5 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp @@ -89,7 +89,7 @@ public: /** * @brief Reset position controller. */ - void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;}; + void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_sv_speed_limit.get(); _stopped = false;}; protected: /** @@ -130,15 +130,15 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_rd_trans_trn_drv, (ParamFloat) _param_rd_trans_drv_trn, - (ParamFloat) _param_ro_max_thr_speed, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_jerk_limit, - (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_sv_max_thr_speed, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_jerk_limit, + (ParamFloat) _param_sv_speed_limit, (ParamFloat) _param_pp_lookahd_gain, (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_pp_lookahd_min, (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_ro_speed_red, - (ParamFloat) _param_ro_speed_th + (ParamFloat) _param_sv_speed_red, + (ParamFloat) _param_sv_speed_th ) }; diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp index 584592331d..1fdd5d24d9 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp @@ -47,12 +47,12 @@ void DifferentialRateControl::updateParams() ModuleParams::updateParams(); // Set up PID controller - _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setGains(_param_sv_yaw_rate_p.get(), _param_sv_yaw_rate_i.get(), 0.f); _pid_yaw_rate.setIntegralLimit(1.f); _pid_yaw_rate.setOutputLimit(1.f); // Set up slew rate - _adjusted_yaw_rate_setpoint.setSlewRate(_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F); + _adjusted_yaw_rate_setpoint.setSlewRate(_param_sv_yaw_accel_limit.get() * M_DEG_TO_RAD_F); } void DifferentialRateControl::updateRateControl() @@ -64,7 +64,7 @@ void DifferentialRateControl::updateRateControl() if (_vehicle_angular_velocity_sub.updated()) { vehicle_angular_velocity_s vehicle_angular_velocity{}; _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); - _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_sv_yaw_rate_th.get() * M_DEG_TO_RAD_F ? vehicle_angular_velocity.xyz[2] : 0.f; } @@ -75,11 +75,11 @@ void DifferentialRateControl::updateRateControl() } if (PX4_ISFINITE(_yaw_rate_setpoint)) { - const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_sv_yaw_rate_th.get() * M_DEG_TO_RAD_F ? _yaw_rate_setpoint : 0.f; const float speed_diff_normalized = SurfaceVehicleControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, - yaw_rate_setpoint, _vehicle_yaw_rate, _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_corr.get(), - _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_sv_max_thr_speed.get(), _param_sv_yaw_rate_corr.get(), + _param_sv_yaw_accel_limit.get() * M_DEG_TO_RAD_F, _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rd_wheel_track.get(), dt); surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; surface_vehicle_steering_setpoint.timestamp = _timestamp; @@ -104,12 +104,12 @@ bool DifferentialRateControl::runSanityChecks() { bool ret = true; - if ((_param_rd_wheel_track.get() < FLT_EPSILON || _param_ro_max_thr_speed.get() < FLT_EPSILON) - && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { + if ((_param_rd_wheel_track.get() < FLT_EPSILON || _param_sv_max_thr_speed.get() < FLT_EPSILON) + && _param_sv_yaw_rate_p.get() < FLT_EPSILON) { ret = false; events::send(events::ID("differential_rate_control_conf_invalid_rate_control"), events::Log::Error, "Invalid configuration for rate control: Neither feed forward nor feedback is setup", _param_rd_wheel_track.get(), - _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_p.get()); + _param_sv_max_thr_speed.get(), _param_sv_yaw_rate_p.get()); } return ret; diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp index 7ca00899a8..3c5373c972 100644 --- a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp @@ -107,12 +107,12 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_ro_yaw_rate_th, - (ParamFloat) _param_ro_yaw_rate_p, - (ParamFloat) _param_ro_yaw_rate_i, - (ParamFloat) _param_ro_yaw_accel_limit, - (ParamFloat) _param_ro_yaw_decel_limit, - (ParamFloat) _param_ro_yaw_rate_corr, - (ParamFloat) _param_ro_max_thr_speed + (ParamFloat) _param_sv_yaw_rate_th, + (ParamFloat) _param_sv_yaw_rate_p, + (ParamFloat) _param_sv_yaw_rate_i, + (ParamFloat) _param_sv_yaw_accel_limit, + (ParamFloat) _param_ro_yaw_decel_limit, + (ParamFloat) _param_sv_yaw_rate_corr, + (ParamFloat) _param_sv_max_thr_speed ) }; diff --git a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp index 652c16afbc..edcc3c9cb8 100644 --- a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp +++ b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp @@ -47,13 +47,13 @@ void DifferentialSpeedControl::updateParams() ModuleParams::updateParams(); // Set up PID controller - _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed.setGains(_param_sv_speed_p.get(), _param_sv_speed_i.get(), 0.f); _pid_speed.setIntegralLimit(1.f); _pid_speed.setOutputLimit(1.f); // Set up slew rate - if (_param_ro_accel_limit.get() > FLT_EPSILON) { - _adjusted_speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); + if (_param_sv_accel_limit.get() > FLT_EPSILON) { + _adjusted_speed_setpoint.setSlewRate(_param_sv_accel_limit.get()); } } @@ -72,8 +72,8 @@ void DifferentialSpeedControl::updateSpeedControl() surface_vehicle_throttle_setpoint.timestamp = _timestamp; surface_vehicle_throttle_setpoint.throttle_body_x = SurfaceVehicleControl::speedControl(_adjusted_speed_setpoint, _pid_speed, - speed_setpoint, _vehicle_speed, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), - _param_ro_max_thr_speed.get(), dt); + speed_setpoint, _vehicle_speed, _param_sv_accel_limit.get(), _param_sv_decel_limit.get(), + _param_sv_max_thr_speed.get(), dt); surface_vehicle_throttle_setpoint.throttle_body_y = 0.f; _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); } @@ -104,7 +104,7 @@ void DifferentialSpeedControl::updateSubscriptions() Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); - _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; + _vehicle_speed = velocity_2d.norm() > _param_sv_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } if (_surface_vehicle_speed_setpoint_sub.updated()) { @@ -117,10 +117,10 @@ void DifferentialSpeedControl::updateSubscriptions() float DifferentialSpeedControl::calcSpeedSetpoint() { - float speed_setpoint = math::constrain(_speed_setpoint, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + float speed_setpoint = math::constrain(_speed_setpoint, -_param_sv_speed_limit.get(), _param_sv_speed_limit.get()); const float speed_setpoint_normalized = math::interpolate(speed_setpoint, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + -_param_sv_max_thr_speed.get(), _param_sv_max_thr_speed.get(), -1.f, 1.f); if (_surface_vehicle_steering_setpoint_sub.updated()) { surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; @@ -131,7 +131,7 @@ float DifferentialSpeedControl::calcSpeedSetpoint() if (fabsf(speed_setpoint_normalized) > 1.f - fabsf( _normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels speed_setpoint = math::interpolate(sign(speed_setpoint_normalized) * (1.f - fabsf(_normalized_speed_diff)), -1.f, - 1.f, -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + 1.f, -_param_sv_max_thr_speed.get(), _param_sv_max_thr_speed.get()); } return speed_setpoint; @@ -141,17 +141,17 @@ bool DifferentialSpeedControl::runSanityChecks() { bool ret = true; - if (_param_ro_speed_limit.get() < FLT_EPSILON) { + if (_param_sv_speed_limit.get() < FLT_EPSILON) { ret = false; events::send(events::ID("differential_speed_control_conf_invalid_speed_lim"), events::Log::Error, - "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + "Invalid configuration of necessary parameter SV_SPEED_LIM", _param_sv_speed_limit.get()); } - if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + if (_param_sv_max_thr_speed.get() < FLT_EPSILON && _param_sv_speed_p.get() < FLT_EPSILON) { ret = false; events::send(events::ID("differential_speed_control_conf_invalid_speed_control"), events::Log::Error, - "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup", - _param_ro_max_thr_speed.get(), _param_ro_speed_p.get()); + "Invalid configuration for speed control: Neither feed forward (SV_MAX_THR_SPEED) nor feedback (SV_SPEED_P) is setup", + _param_sv_max_thr_speed.get(), _param_sv_speed_p.get()); } return ret; diff --git a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp index f7d245301f..aab20b9bf1 100644 --- a/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp +++ b/src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp @@ -125,13 +125,13 @@ private: SlewRate _adjusted_speed_setpoint{0.f}; DEFINE_PARAMETERS( - (ParamFloat) _param_ro_max_thr_speed, - (ParamFloat) _param_ro_speed_p, - (ParamFloat) _param_ro_speed_i, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_jerk_limit, - (ParamFloat) _param_ro_speed_limit, - (ParamFloat) _param_ro_speed_th + (ParamFloat) _param_sv_max_thr_speed, + (ParamFloat) _param_sv_speed_p, + (ParamFloat) _param_sv_speed_i, + (ParamFloat) _param_sv_accel_limit, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_jerk_limit, + (ParamFloat) _param_sv_speed_limit, + (ParamFloat) _param_sv_speed_th ) }; diff --git a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp index f18c8621a0..08a538a0c0 100644 --- a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp +++ b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp @@ -44,9 +44,9 @@ void MecanumActControl::updateParams() { ModuleParams::updateParams(); - if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { - _adjusted_throttle_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); - _adjusted_throttle_y_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + if (_param_sv_accel_limit.get() > FLT_EPSILON && _param_sv_max_thr_speed.get() > FLT_EPSILON) { + _adjusted_throttle_x_setpoint.setSlewRate(_param_sv_accel_limit.get() / _param_sv_max_thr_speed.get()); + _adjusted_throttle_y_setpoint.setSlewRate(_param_sv_accel_limit.get() / _param_sv_max_thr_speed.get()); } } @@ -76,11 +76,11 @@ void MecanumActControl::updateActControl() const float current_throttle_x = (actuator_motors_sub.control[0] + actuator_motors_sub.control[1]) / 2.f; const float current_throttle_y = (actuator_motors_sub.control[2] - actuator_motors_sub.control[0]) / 2.f; const float adjusted_throttle_x_setpoint = SurfaceVehicleControl::throttleControl(_adjusted_throttle_x_setpoint, - _throttle_x_setpoint, current_throttle_x, _param_ro_accel_limit.get(), - _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); + _throttle_x_setpoint, current_throttle_x, _param_sv_accel_limit.get(), + _param_sv_decel_limit.get(), _param_sv_max_thr_speed.get(), dt); const float adjusted_throttle_y_setpoint = SurfaceVehicleControl::throttleControl(_adjusted_throttle_y_setpoint, - _throttle_y_setpoint, current_throttle_y, _param_ro_accel_limit.get(), - _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), dt); + _throttle_y_setpoint, current_throttle_y, _param_sv_accel_limit.get(), + _param_sv_decel_limit.get(), _param_sv_max_thr_speed.get(), dt); actuator_motors_s actuator_motors{}; actuator_motors.reversible_flags = _param_r_rev.get(); computeInverseKinematics(adjusted_throttle_x_setpoint, adjusted_throttle_y_setpoint, diff --git a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp index 584ea6fb2f..dc087e24ec 100644 --- a/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp +++ b/src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp @@ -108,8 +108,8 @@ private: // Parameters DEFINE_PARAMETERS( (ParamInt) _param_r_rev, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_max_thr_speed + (ParamFloat) _param_sv_accel_limit, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_max_thr_speed ) }; diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp index 5c7659fecf..cd58f83dee 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp @@ -46,12 +46,12 @@ void MecanumAttControl::updateParams() { ModuleParams::updateParams(); - if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + if (_param_sv_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; } // Set up PID controller - _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setGains(_param_sv_yaw_p.get(), 0.f, 0.f); _pid_yaw.setIntegralLimit(_max_yaw_rate); _pid_yaw.setOutputLimit(_max_yaw_rate); @@ -101,14 +101,14 @@ bool MecanumAttControl::runSanityChecks() { bool ret = true; - if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + if (_param_sv_yaw_rate_limit.get() < FLT_EPSILON) { ret = false; } - if (_param_ro_yaw_p.get() < FLT_EPSILON) { + if (_param_sv_yaw_p.get() < FLT_EPSILON) { ret = false; events::send(events::ID("mecanum_att_control_conf_invalid_yaw_p"), events::Log::Error, - "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get()); + "Invalid configuration of necessary parameter SV_YAW_P", _param_sv_yaw_p.get()); } return ret; diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp index 6a8f3f5518..f102a82e03 100644 --- a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp @@ -109,8 +109,8 @@ private: // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_ro_yaw_rate_limit, - (ParamFloat) _param_ro_yaw_p, - (ParamFloat) _param_ro_yaw_stick_dz + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_p, + (ParamFloat) _param_sv_yaw_stick_dz ) }; diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp index 091e8cd21b..954b227f79 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp @@ -77,7 +77,7 @@ void MecanumAutoMode::autoControl() // Waypoint cruising speed float cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( - position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + position_setpoint_triplet.current.cruising_speed, 0.f, _param_sv_speed_limit.get()) : _param_sv_speed_limit.get(); surface_vehicle_position_setpoint_s surface_vehicle_position_setpoint{}; surface_vehicle_position_setpoint.timestamp = hrt_absolute_time(); @@ -86,7 +86,7 @@ void MecanumAutoMode::autoControl() surface_vehicle_position_setpoint.start_ned[0] = prev_wp_ned(0); surface_vehicle_position_setpoint.start_ned[1] = prev_wp_ned(1); surface_vehicle_position_setpoint.arrival_speed = arrivalSpeed(cruising_speed, waypoint_transition_angle, - _param_ro_speed_limit.get(), _param_ro_speed_red.get(), curr_wp_type); + _param_sv_speed_limit.get(), _param_sv_speed_red.get(), curr_wp_type); surface_vehicle_position_setpoint.cruising_speed = cruising_speed; surface_vehicle_position_setpoint.yaw = PX4_ISFINITE(position_setpoint_triplet.current.yaw) ? position_setpoint_triplet.current.yaw : NAN; diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp index 8fe904d4bc..28443b33dc 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp @@ -93,7 +93,7 @@ private: uORB::Publication _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)}; DEFINE_PARAMETERS( - (ParamFloat) _param_ro_speed_limit, - (ParamFloat) _param_ro_speed_red + (ParamFloat) _param_sv_speed_limit, + (ParamFloat) _param_sv_speed_red ) }; diff --git a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp index 9bf7f3c38b..227dda045e 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp @@ -49,7 +49,7 @@ MecanumManualMode::MecanumManualMode(ModuleParams *parent) : ModuleParams(parent void MecanumManualMode::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; } void MecanumManualMode::manual() @@ -59,7 +59,7 @@ void MecanumManualMode::manual() surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; surface_vehicle_steering_setpoint.timestamp = hrt_absolute_time(); surface_vehicle_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()); + (manual_control_setpoint.yaw, _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); _surface_vehicle_steering_setpoint_pub.publish(surface_vehicle_steering_setpoint); surface_vehicle_throttle_setpoint_s surface_vehicle_throttle_setpoint{}; surface_vehicle_throttle_setpoint.timestamp = hrt_absolute_time(); @@ -80,7 +80,7 @@ void MecanumManualMode::acro() surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); surface_vehicle_rate_setpoint.yaw_rate_setpoint = _max_yaw_rate * math::superexpo(manual_control_setpoint.yaw, - _param_ro_yaw_expo.get(), _param_ro_yaw_supexpo.get()); + _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); } @@ -108,7 +108,7 @@ void MecanumManualMode::stab() surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); surface_vehicle_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()); + manual_control_setpoint.yaw, _param_sv_yaw_stick_dz.get()), _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_rate_setpoint); // Set uncontrolled setpoint invalid @@ -149,9 +149,9 @@ void MecanumManualMode::position() Vector3f velocity_setpoint_body{}; velocity_setpoint_body(0) = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + -1.f, 1.f, -_param_sv_speed_limit.get(), _param_sv_speed_limit.get()); velocity_setpoint_body(1) = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + -1.f, 1.f, -_param_sv_speed_limit.get(), _param_sv_speed_limit.get()); velocity_setpoint_body(2) = 0.f; if (fabsf(manual_control_setpoint.yaw) > FLT_EPSILON || velocity_setpoint_body.norm() < FLT_EPSILON) { @@ -168,7 +168,7 @@ void MecanumManualMode::position() surface_vehicle_rate_setpoint_s surface_vehicle_rate_setpoint{}; surface_vehicle_rate_setpoint.timestamp = hrt_absolute_time(); surface_vehicle_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()); + manual_control_setpoint.yaw, _param_sv_yaw_stick_dz.get()), _param_sv_yaw_expo.get(), _param_sv_yaw_supexpo.get()); _surface_vehicle_rate_setpoint_pub.publish(surface_vehicle_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 e726f258ed..4d70a646e3 100644 --- a/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp +++ b/src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp @@ -124,13 +124,13 @@ private: float _max_yaw_rate{NAN}; 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_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_stick_dz, + (ParamFloat) _param_sv_yaw_expo, + (ParamFloat) _param_sv_yaw_supexpo, (ParamFloat) _param_rm_yaw_stk_gain, (ParamFloat) _param_pp_lookahd_max, - (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_sv_speed_limit, (ParamFloat) _param_rm_course_ctl_th ) }; diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp index d6515a1a33..f74dfd61da 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp @@ -47,7 +47,7 @@ MecanumPosControl::MecanumPosControl(ModuleParams *parent) : ModuleParams(parent void MecanumPosControl::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_rate = _param_sv_yaw_rate_limit.get() * M_DEG_TO_RAD_F; } @@ -68,8 +68,8 @@ void MecanumPosControl::updatePosControl() if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) { - float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), - _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); + float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_sv_jerk_limit.get(), + _param_sv_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); speed_setpoint = math::min(speed_setpoint, _cruising_speed); pure_pursuit_status_s pure_pursuit_status{}; @@ -135,7 +135,7 @@ void MecanumPosControl::updateSubscriptions() Vector3f velocity_ned(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); Vector3f velocity_xyz = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_ned); Vector2f velocity_2d = Vector2f(velocity_xyz(0), velocity_xyz(1)); - _vehicle_speed = velocity_2d.norm() > _param_ro_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; + _vehicle_speed = velocity_2d.norm() > _param_sv_speed_th.get() ? sign(velocity_2d(0)) * velocity_2d.norm() : 0.f; } if (_surface_vehicle_position_setpoint_sub.updated()) { @@ -147,7 +147,7 @@ void MecanumPosControl::updateSubscriptions() surface_vehicle_position_setpoint.arrival_speed : 0.f; _cruising_speed = PX4_ISFINITE(surface_vehicle_position_setpoint.cruising_speed) ? surface_vehicle_position_setpoint.cruising_speed : - _param_ro_speed_limit.get(); + _param_sv_speed_limit.get(); _target_waypoint_ned = Vector2f(surface_vehicle_position_setpoint.position_ned[0], surface_vehicle_position_setpoint.position_ned[1]); _stopped = false; @@ -158,15 +158,15 @@ bool MecanumPosControl::runSanityChecks() { bool ret = true; - if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + if (_param_sv_yaw_rate_limit.get() < FLT_EPSILON) { ret = false; } - if (_param_ro_speed_limit.get() < FLT_EPSILON) { + if (_param_sv_speed_limit.get() < FLT_EPSILON) { ret = false; } - if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + if (_param_sv_max_thr_speed.get() < FLT_EPSILON && _param_sv_speed_p.get() < FLT_EPSILON) { ret = false; } diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp index 301da6791e..d0986f0b08 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp @@ -82,7 +82,7 @@ public: /** * @brief Reset position controller. */ - void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_ro_speed_limit.get(); _stopped = false;}; + void reset() {_start_ned = Vector2f{NAN, NAN}; _target_waypoint_ned = Vector2f{NAN, NAN}; _arrival_speed = 0.f; _cruising_speed = _param_sv_speed_limit.get(); _stopped = false;}; protected: @@ -125,20 +125,20 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_rm_course_ctl_th, - (ParamFloat) _param_ro_max_thr_speed, - (ParamFloat) _param_ro_speed_p, - (ParamFloat) _param_ro_speed_i, - (ParamFloat) _param_ro_yaw_stick_dz, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_jerk_limit, - (ParamFloat) _param_ro_speed_limit, - (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_sv_max_thr_speed, + (ParamFloat) _param_sv_speed_p, + (ParamFloat) _param_sv_speed_i, + (ParamFloat) _param_sv_yaw_stick_dz, + (ParamFloat) _param_sv_accel_limit, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_jerk_limit, + (ParamFloat) _param_sv_speed_limit, + (ParamFloat) _param_sv_speed_th, (ParamFloat) _param_pp_lookahd_gain, (ParamFloat) _param_pp_lookahd_max, (ParamFloat) _param_pp_lookahd_min, - (ParamFloat) _param_ro_yaw_rate_limit, - (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_p, (ParamFloat) _param_nav_acc_rad ) }; diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp index 6aa80e06c0..5f005e8021 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp @@ -47,12 +47,12 @@ void MecanumRateControl::updateParams() ModuleParams::updateParams(); // Set up PID controller - _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setGains(_param_sv_yaw_rate_p.get(), _param_sv_yaw_rate_i.get(), 0.f); _pid_yaw_rate.setIntegralLimit(1.f); _pid_yaw_rate.setOutputLimit(1.f); // Set up slew rate - _adjusted_yaw_rate_setpoint.setSlewRate(_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F); + _adjusted_yaw_rate_setpoint.setSlewRate(_param_sv_yaw_accel_limit.get() * M_DEG_TO_RAD_F); } void MecanumRateControl::updateRateControl() @@ -64,7 +64,7 @@ void MecanumRateControl::updateRateControl() if (_vehicle_angular_velocity_sub.updated()) { vehicle_angular_velocity_s vehicle_angular_velocity{}; _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); - _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_sv_yaw_rate_th.get() * M_DEG_TO_RAD_F ? vehicle_angular_velocity.xyz[2] : 0.f; } @@ -75,11 +75,11 @@ void MecanumRateControl::updateRateControl() } if (PX4_ISFINITE(_yaw_rate_setpoint)) { - const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + const float yaw_rate_setpoint = fabsf(_yaw_rate_setpoint) > _param_sv_yaw_rate_th.get() * M_DEG_TO_RAD_F ? _yaw_rate_setpoint : 0.f; const float speed_diff_normalized = SurfaceVehicleControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, - yaw_rate_setpoint, _vehicle_yaw_rate, _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_corr.get(), - _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_sv_max_thr_speed.get(), _param_sv_yaw_rate_corr.get(), + _param_sv_yaw_accel_limit.get() * M_DEG_TO_RAD_F, _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F, _param_rm_wheel_track.get(), dt); surface_vehicle_steering_setpoint_s surface_vehicle_steering_setpoint{}; surface_vehicle_steering_setpoint.timestamp = _timestamp; @@ -104,19 +104,19 @@ bool MecanumRateControl::runSanityChecks() { bool ret = true; - if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + if (_param_sv_yaw_rate_limit.get() < FLT_EPSILON) { ret = false; events::send(events::ID("mecanum_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, - "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); + "Invalid configuration of necessary parameter SV_YAW_RATE_LIM", _param_sv_yaw_rate_limit.get()); } - if ((_param_rm_wheel_track.get() < FLT_EPSILON || _param_ro_max_thr_speed.get() < FLT_EPSILON) - && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { + if ((_param_rm_wheel_track.get() < FLT_EPSILON || _param_sv_max_thr_speed.get() < FLT_EPSILON) + && _param_sv_yaw_rate_p.get() < FLT_EPSILON) { ret = false; events::send(events::ID("mecanum_rate_control_conf_invalid_rate_control"), events::Log::Error, - "Invalid configuration for rate control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_YAW_RATE_P) is setup", - _param_rm_wheel_track.get(), _param_ro_max_thr_speed.get(), _param_ro_yaw_rate_p.get()); + "Invalid configuration for rate control: Neither feed forward (SV_MAX_THR_SPEED) nor feedback (SV_YAW_RATE_P) is setup", + _param_rm_wheel_track.get(), _param_sv_max_thr_speed.get(), _param_sv_yaw_rate_p.get()); } return ret; diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp index 199a6cf8b7..9c4eb31fa8 100644 --- a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp @@ -107,13 +107,13 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_rm_wheel_track, - (ParamFloat) _param_ro_max_thr_speed, - (ParamFloat) _param_ro_yaw_rate_limit, - (ParamFloat) _param_ro_yaw_rate_th, - (ParamFloat) _param_ro_yaw_rate_p, - (ParamFloat) _param_ro_yaw_rate_i, - (ParamFloat) _param_ro_yaw_accel_limit, - (ParamFloat) _param_ro_yaw_decel_limit, - (ParamFloat) _param_ro_yaw_rate_corr + (ParamFloat) _param_sv_max_thr_speed, + (ParamFloat) _param_sv_yaw_rate_limit, + (ParamFloat) _param_sv_yaw_rate_th, + (ParamFloat) _param_sv_yaw_rate_p, + (ParamFloat) _param_sv_yaw_rate_i, + (ParamFloat) _param_sv_yaw_accel_limit, + (ParamFloat) _param_ro_yaw_decel_limit, + (ParamFloat) _param_sv_yaw_rate_corr ) }; diff --git a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp index 9ce3d1d0aa..686f9a8997 100644 --- a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp +++ b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp @@ -47,17 +47,17 @@ void MecanumSpeedControl::updateParams() ModuleParams::updateParams(); // Set up PID controllers - _pid_speed_x.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed_x.setGains(_param_sv_speed_p.get(), _param_sv_speed_i.get(), 0.f); _pid_speed_x.setIntegralLimit(1.f); _pid_speed_x.setOutputLimit(1.f); - _pid_speed_y.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed_y.setGains(_param_sv_speed_p.get(), _param_sv_speed_i.get(), 0.f); _pid_speed_y.setIntegralLimit(1.f); _pid_speed_y.setOutputLimit(1.f); // Set up slew rates - if (_param_ro_accel_limit.get() > FLT_EPSILON) { - _adjusted_speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get()); - _adjusted_speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get()); + if (_param_sv_accel_limit.get() > FLT_EPSILON) { + _adjusted_speed_x_setpoint.setSlewRate(_param_sv_accel_limit.get()); + _adjusted_speed_y_setpoint.setSlewRate(_param_sv_accel_limit.get()); } } @@ -77,12 +77,12 @@ void MecanumSpeedControl::updateSpeedControl() surface_vehicle_throttle_setpoint.throttle_body_x = SurfaceVehicleControl::speedControl(_adjusted_speed_x_setpoint, _pid_speed_x, - speed_setpoint(0), _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), - _param_ro_max_thr_speed.get(), dt); + speed_setpoint(0), _vehicle_speed_body_x, _param_sv_accel_limit.get(), _param_sv_decel_limit.get(), + _param_sv_max_thr_speed.get(), dt); surface_vehicle_throttle_setpoint.throttle_body_y = SurfaceVehicleControl::speedControl(_adjusted_speed_y_setpoint, _pid_speed_y, - speed_setpoint(1), _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), - _param_ro_max_thr_speed.get(), dt); + speed_setpoint(1), _vehicle_speed_body_y, _param_sv_accel_limit.get(), _param_sv_decel_limit.get(), + _param_sv_max_thr_speed.get(), dt); _surface_vehicle_throttle_setpoint_pub.publish(surface_vehicle_throttle_setpoint); } @@ -113,8 +113,8 @@ void MecanumSpeedControl::updateSubscriptions() _vehicle_local_position_sub.copy(&vehicle_local_position); const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; - _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_sv_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_sv_speed_th.get() ? velocity_in_body_frame(1) : 0.f; } if (_surface_vehicle_speed_setpoint_sub.updated()) { @@ -136,10 +136,10 @@ Vector2f MecanumSpeedControl::calcSpeedSetpoint() Vector2f speed_setpoint = Vector2f(_speed_x_setpoint, _speed_y_setpoint); float speed_x_setpoint_normalized = math::interpolate(_speed_x_setpoint, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + -_param_sv_max_thr_speed.get(), _param_sv_max_thr_speed.get(), -1.f, 1.f); float speed_y_setpoint_normalized = math::interpolate(_speed_y_setpoint, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + -_param_sv_max_thr_speed.get(), _param_sv_max_thr_speed.get(), -1.f, 1.f); const float total_speed = fabsf(speed_x_setpoint_normalized) + fabsf(speed_y_setpoint_normalized) + fabsf( _normalized_speed_diff); @@ -152,13 +152,13 @@ Vector2f MecanumSpeedControl::calcSpeedSetpoint() speed_x_setpoint_normalized *= magnitude * normalization; speed_y_setpoint_normalized *= magnitude * normalization; speed_setpoint(0) = math::interpolate(speed_x_setpoint_normalized, -1.f, 1.f, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + -_param_sv_max_thr_speed.get(), _param_sv_max_thr_speed.get()); speed_setpoint(1) = math::interpolate(speed_y_setpoint_normalized, -1.f, 1.f, - -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + -_param_sv_max_thr_speed.get(), _param_sv_max_thr_speed.get()); } - speed_setpoint(0) = fabsf(speed_setpoint(0)) > _param_ro_speed_th.get() ? speed_setpoint(0) : 0.f; - speed_setpoint(1) = fabsf(speed_setpoint(1)) > _param_ro_speed_th.get() ? speed_setpoint(1) : 0.f; + speed_setpoint(0) = fabsf(speed_setpoint(0)) > _param_sv_speed_th.get() ? speed_setpoint(0) : 0.f; + speed_setpoint(1) = fabsf(speed_setpoint(1)) > _param_sv_speed_th.get() ? speed_setpoint(1) : 0.f; return speed_setpoint; } @@ -167,19 +167,19 @@ bool MecanumSpeedControl::runSanityChecks() { bool ret = true; - if (_param_ro_speed_limit.get() < FLT_EPSILON) { + if (_param_sv_speed_limit.get() < FLT_EPSILON) { ret = false; events::send(events::ID("mecanum_vel_control_conf_invalid_speed_lim"), events::Log::Error, - "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + "Invalid configuration of necessary parameter SV_SPEED_LIM", _param_sv_speed_limit.get()); } - if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + if (_param_sv_max_thr_speed.get() < FLT_EPSILON && _param_sv_speed_p.get() < FLT_EPSILON) { ret = false; events::send(events::ID("mecanum_vel_control_conf_invalid_speed_control"), events::Log::Error, - "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup", - _param_ro_max_thr_speed.get(), - _param_ro_speed_p.get()); + "Invalid configuration for speed control: Neither feed forward (SV_MAX_THR_SPEED) nor feedback (SV_SPEED_P) is setup", + _param_sv_max_thr_speed.get(), + _param_sv_speed_p.get()); } return ret; diff --git a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp index 25db02a7d8..9843c92cc5 100644 --- a/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp +++ b/src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp @@ -130,13 +130,13 @@ private: SlewRate _adjusted_speed_y_setpoint{0.f}; DEFINE_PARAMETERS( - (ParamFloat) _param_ro_max_thr_speed, - (ParamFloat) _param_ro_speed_p, - (ParamFloat) _param_ro_speed_i, - (ParamFloat) _param_ro_accel_limit, - (ParamFloat) _param_ro_decel_limit, - (ParamFloat) _param_ro_jerk_limit, - (ParamFloat) _param_ro_speed_limit, - (ParamFloat) _param_ro_speed_th + (ParamFloat) _param_sv_max_thr_speed, + (ParamFloat) _param_sv_speed_p, + (ParamFloat) _param_sv_speed_i, + (ParamFloat) _param_sv_accel_limit, + (ParamFloat) _param_sv_decel_limit, + (ParamFloat) _param_sv_jerk_limit, + (ParamFloat) _param_sv_speed_limit, + (ParamFloat) _param_sv_speed_th ) };