fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 14:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sv: rename rover parameters to surface vehicle

Browse Source
This commit is contained in:
chfriedrich98
2025-09-05 13:48:17 +02:00
parent fb05dedb7b
commit 9c38602c12
44 changed files with 338 additions and 338 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/rtl/rtl_time_estimator.cpp
+1 -1
View File
@@ -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
src/lib/surface_vehicle_control/rovercontrol_params.c → src/lib/surface_vehicle_control/surface_vehicle_params.c
+50 -50
View File
@@ -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);
src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp
+4 -4
View File
@@ -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);
src/modules/rover_ackermann/AckermannActControl/AckermannActControl.hpp
+3 -3
View File
@@ -104,8 +104,8 @@ private:
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(ParamFloat<px4::params::RA_STR_RATE_LIM>) _param_ra_str_rate_limit,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
(ParamFloat<px4::params::SV_ACCEL_LIM>) _param_sv_accel_limit,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed
)
};
src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp
+6 -6
View File
@@ -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<float> (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;
}
src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp
+5 -5
View File
@@ -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<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_P>) _param_sv_yaw_p,
(ParamFloat<px4::params::SV_YAW_STICK_DZ>) _param_sv_yaw_stick_dz
)
};
src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp
+5 -5
View File
@@ -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
}
src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp
+4 -4
View File
@@ -127,14 +127,14 @@ private:
int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
(ParamFloat<px4::params::SV_SPEED_RED>) _param_sv_speed_red,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed
)
};
src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.cpp
+5 -5
View File
@@ -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<float>
(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<float>(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<float>(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<float>(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
src/modules/rover_ackermann/AckermannDriveModes/AckermannManualMode/AckermannManualMode.hpp
+6 -6
View File
@@ -123,12 +123,12 @@ private:
float _max_yaw_rate{NAN};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz,
(ParamFloat<px4::params::RO_YAW_EXPO>) _param_ro_yaw_expo,
(ParamFloat<px4::params::RO_YAW_SUPEXPO>) _param_ro_yaw_supexpo,
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_P>) _param_sv_yaw_p,
(ParamFloat<px4::params::SV_YAW_STICK_DZ>) _param_sv_yaw_stick_dz,
(ParamFloat<px4::params::SV_YAW_EXPO>) _param_sv_yaw_expo,
(ParamFloat<px4::params::SV_YAW_SUPEXPO>) _param_sv_yaw_supexpo,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit
)
};
src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp
+10 -10
View File
@@ -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;
}
src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp
+8 -8
View File
@@ -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<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::SV_SPEED_RED>) _param_sv_speed_red,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_JERK_LIM>) _param_sv_jerk_limit,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
(ParamFloat<px4::params::SV_SPEED_TH>) _param_sv_speed_th
)
};
src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp
+12 -12
View File
@@ -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<float>(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<float>(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<float>(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());
}
src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp
+9 -9
View File
@@ -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<float> _adjusted_yaw_rate_setpoint{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_RATE_TH>) _param_ro_yaw_rate_th,
(ParamFloat<px4::params::RO_YAW_RATE_P>) _param_ro_yaw_rate_p,
(ParamFloat<px4::params::RO_YAW_RATE_I>) _param_ro_yaw_rate_i,
(ParamFloat<px4::params::RO_YAW_ACCEL_LIM>) _param_ro_yaw_accel_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::RO_YAW_RATE_CORR>) _param_ro_yaw_rate_corr
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_RATE_TH>) _param_sv_yaw_rate_th,
(ParamFloat<px4::params::SV_YAW_RATE_P>) _param_sv_yaw_rate_p,
(ParamFloat<px4::params::SV_YAW_RATE_I>) _param_sv_yaw_rate_i,
(ParamFloat<px4::params::SV_YAW_ACCEL_LIM>) _param_sv_yaw_accel_limit,
(ParamFloat<px4::params::SV_SPEED_TH>) _param_sv_speed_th,
(ParamFloat<px4::params::SV_YAW_RATE_CORR>) _param_sv_yaw_rate_corr
)
};
src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.cpp
+11 -11
View File
@@ -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<float>(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());
}
src/modules/rover_ackermann/AckermannSpeedControl/AckermannSpeedControl.hpp
+8 -8
View File
@@ -116,13 +116,13 @@ private:
SlewRate<float> _adjusted_speed_setpoint{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_SPEED_P>) _param_ro_speed_p,
(ParamFloat<px4::params::RO_SPEED_I>) _param_ro_speed_i,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::SV_SPEED_P>) _param_sv_speed_p,
(ParamFloat<px4::params::SV_SPEED_I>) _param_sv_speed_i,
(ParamFloat<px4::params::SV_ACCEL_LIM>) _param_sv_accel_limit,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_JERK_LIM>) _param_sv_jerk_limit,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::SV_SPEED_TH>) _param_sv_speed_th
)
};
src/modules/rover_differential/DifferentialActControl/DifferentialActControl.cpp
+4 -4
View File
@@ -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);
src/modules/rover_differential/DifferentialActControl/DifferentialActControl.hpp
+3 -3
View File
@@ -106,8 +106,8 @@ private:
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
(ParamFloat<px4::params::SV_ACCEL_LIM>) _param_sv_accel_limit,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed
)
};
src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp
+6 -6
View File
@@ -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<float>(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;
src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp
+3 -3
View File
@@ -108,8 +108,8 @@ private:
// Parameters
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_P>) _param_sv_yaw_p,
(ParamFloat<px4::params::SV_YAW_STICK_DZ>) _param_sv_yaw_stick_dz
)
};
src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp
+2 -2
View File
@@ -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);
src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp
+2 -2
View File
@@ -94,8 +94,8 @@ private:
uORB::Publication<surface_vehicle_position_setpoint_s> _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::SV_SPEED_RED>) _param_sv_speed_red,
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn
)
};
src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.cpp
+6 -6
View File
@@ -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<float>
(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<float>(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<float>(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<float>(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<float>(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
src/modules/rover_differential/DifferentialDriveModes/DifferentialManualMode/DifferentialManualMode.hpp
+5 -5
View File
@@ -122,12 +122,12 @@ private:
float _max_yaw_rate{NAN};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz,
(ParamFloat<px4::params::RO_YAW_EXPO>) _param_ro_yaw_expo,
(ParamFloat<px4::params::RO_YAW_SUPEXPO>) _param_ro_yaw_supexpo,
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_STICK_DZ>) _param_sv_yaw_stick_dz,
(ParamFloat<px4::params::SV_YAW_EXPO>) _param_sv_yaw_expo,
(ParamFloat<px4::params::SV_YAW_SUPEXPO>) _param_sv_yaw_supexpo,
(ParamFloat<px4::params::RD_YAW_STK_GAIN>) _param_rd_yaw_stk_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit
)
};
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp
+9 -9
View File
@@ -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;
}
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp
+7 -7
View File
@@ -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<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_JERK_LIM>) _param_sv_jerk_limit,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
(ParamFloat<px4::params::SV_SPEED_RED>) _param_sv_speed_red,
(ParamFloat<px4::params::SV_SPEED_TH>) _param_sv_speed_th
)
};
src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp
+9 -9
View File
@@ -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<float, float, float>(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;
src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp
+7 -7
View File
@@ -107,12 +107,12 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RO_YAW_RATE_TH>) _param_ro_yaw_rate_th,
(ParamFloat<px4::params::RO_YAW_RATE_P>) _param_ro_yaw_rate_p,
(ParamFloat<px4::params::RO_YAW_RATE_I>) _param_ro_yaw_rate_i,
(ParamFloat<px4::params::RO_YAW_ACCEL_LIM>) _param_ro_yaw_accel_limit,
(ParamFloat<px4::params::RO_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit,
(ParamFloat<px4::params::RO_YAW_RATE_CORR>) _param_ro_yaw_rate_corr,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
(ParamFloat<px4::params::SV_YAW_RATE_TH>) _param_sv_yaw_rate_th,
(ParamFloat<px4::params::SV_YAW_RATE_P>) _param_sv_yaw_rate_p,
(ParamFloat<px4::params::SV_YAW_RATE_I>) _param_sv_yaw_rate_i,
(ParamFloat<px4::params::SV_YAW_ACCEL_LIM>) _param_sv_yaw_accel_limit,
(ParamFloat<px4::params::SV_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit,
(ParamFloat<px4::params::SV_YAW_RATE_CORR>) _param_sv_yaw_rate_corr,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed
)
};
src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.cpp
+14 -14
View File
@@ -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<float>(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<float>(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<float>(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<float, float>(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;
src/modules/rover_differential/DifferentialSpeedControl/DifferentialSpeedControl.hpp
+8 -8
View File
@@ -125,13 +125,13 @@ private:
SlewRate<float> _adjusted_speed_setpoint{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_SPEED_P>) _param_ro_speed_p,
(ParamFloat<px4::params::RO_SPEED_I>) _param_ro_speed_i,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::SV_SPEED_P>) _param_sv_speed_p,
(ParamFloat<px4::params::SV_SPEED_I>) _param_sv_speed_i,
(ParamFloat<px4::params::SV_ACCEL_LIM>) _param_sv_accel_limit,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_JERK_LIM>) _param_sv_jerk_limit,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::SV_SPEED_TH>) _param_sv_speed_th
)
};
src/modules/rover_mecanum/MecanumActControl/MecanumActControl.cpp
+7 -7
View File
@@ -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,
src/modules/rover_mecanum/MecanumActControl/MecanumActControl.hpp
+3 -3
View File
@@ -108,8 +108,8 @@ private:
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
(ParamFloat<px4::params::SV_ACCEL_LIM>) _param_sv_accel_limit,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed
)
};
src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp
+6 -6
View File
@@ -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<float>(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;
src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp
+3 -3
View File
@@ -109,8 +109,8 @@ private:
// Parameters
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_P>) _param_sv_yaw_p,
(ParamFloat<px4::params::SV_YAW_STICK_DZ>) _param_sv_yaw_stick_dz
)
};
src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp
+2 -2
View File
@@ -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;
src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp
+2 -2
View File
@@ -93,7 +93,7 @@ private:
uORB::Publication<surface_vehicle_position_setpoint_s> _surface_vehicle_position_setpoint_pub{ORB_ID(surface_vehicle_position_setpoint)};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::SV_SPEED_RED>) _param_sv_speed_red
)
};
src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.cpp
+7 -7
View File
@@ -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<float>
(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<float>(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<float>(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<float>(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<float>(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<float>(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
src/modules/rover_mecanum/MecanumDriveModes/MecanumManualMode/MecanumManualMode.hpp
+5 -5
View File
@@ -124,13 +124,13 @@ private:
float _max_yaw_rate{NAN};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz,
(ParamFloat<px4::params::RO_YAW_EXPO>) _param_ro_yaw_expo,
(ParamFloat<px4::params::RO_YAW_SUPEXPO>) _param_ro_yaw_supexpo,
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_STICK_DZ>) _param_sv_yaw_stick_dz,
(ParamFloat<px4::params::SV_YAW_EXPO>) _param_sv_yaw_expo,
(ParamFloat<px4::params::SV_YAW_SUPEXPO>) _param_sv_yaw_supexpo,
(ParamFloat<px4::params::RM_YAW_STK_GAIN>) _param_rm_yaw_stk_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::RM_COURSE_CTL_TH>) _param_rm_course_ctl_th
)
};
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp
+8 -8
View File
@@ -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;
}
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp
+12 -12
View File
@@ -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<px4::params::RM_COURSE_CTL_TH>) _param_rm_course_ctl_th,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_SPEED_P>) _param_ro_speed_p,
(ParamFloat<px4::params::RO_SPEED_I>) _param_ro_speed_i,
(ParamFloat<px4::params::RO_YAW_STICK_DZ>) _param_ro_yaw_stick_dz,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::SV_SPEED_P>) _param_sv_speed_p,
(ParamFloat<px4::params::SV_SPEED_I>) _param_sv_speed_i,
(ParamFloat<px4::params::SV_YAW_STICK_DZ>) _param_sv_yaw_stick_dz,
(ParamFloat<px4::params::SV_ACCEL_LIM>) _param_sv_accel_limit,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_JERK_LIM>) _param_sv_jerk_limit,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::SV_SPEED_TH>) _param_sv_speed_th,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_P>) _param_sv_yaw_p,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
)
};
src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp
+12 -12
View File
@@ -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<float>(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<float, float, float>(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;
src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp
+8 -8
View File
@@ -107,13 +107,13 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RM_WHEEL_TRACK>) _param_rm_wheel_track,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_RATE_TH>) _param_ro_yaw_rate_th,
(ParamFloat<px4::params::RO_YAW_RATE_P>) _param_ro_yaw_rate_p,
(ParamFloat<px4::params::RO_YAW_RATE_I>) _param_ro_yaw_rate_i,
(ParamFloat<px4::params::RO_YAW_ACCEL_LIM>) _param_ro_yaw_accel_limit,
(ParamFloat<px4::params::RO_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit,
(ParamFloat<px4::params::RO_YAW_RATE_CORR>) _param_ro_yaw_rate_corr
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::SV_YAW_RATE_LIM>) _param_sv_yaw_rate_limit,
(ParamFloat<px4::params::SV_YAW_RATE_TH>) _param_sv_yaw_rate_th,
(ParamFloat<px4::params::SV_YAW_RATE_P>) _param_sv_yaw_rate_p,
(ParamFloat<px4::params::SV_YAW_RATE_I>) _param_sv_yaw_rate_i,
(ParamFloat<px4::params::SV_YAW_ACCEL_LIM>) _param_sv_yaw_accel_limit,
(ParamFloat<px4::params::SV_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit,
(ParamFloat<px4::params::SV_YAW_RATE_CORR>) _param_sv_yaw_rate_corr
)
};
src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.cpp
+23 -23
View File
@@ -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<float>(_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<float>(_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<float>(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<float>(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<float>(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<float, float>(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;
src/modules/rover_mecanum/MecanumSpeedControl/MecanumSpeedControl.hpp
+8 -8
View File
@@ -130,13 +130,13 @@ private:
SlewRate<float> _adjusted_speed_y_setpoint{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_SPEED_P>) _param_ro_speed_p,
(ParamFloat<px4::params::RO_SPEED_I>) _param_ro_speed_i,
(ParamFloat<px4::params::RO_ACCEL_LIM>) _param_ro_accel_limit,
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
(ParamFloat<px4::params::SV_MAX_THR_SPEED>) _param_sv_max_thr_speed,
(ParamFloat<px4::params::SV_SPEED_P>) _param_sv_speed_p,
(ParamFloat<px4::params::SV_SPEED_I>) _param_sv_speed_i,
(ParamFloat<px4::params::SV_ACCEL_LIM>) _param_sv_accel_limit,
(ParamFloat<px4::params::SV_DECEL_LIM>) _param_sv_decel_limit,
(ParamFloat<px4::params::SV_JERK_LIM>) _param_sv_jerk_limit,
(ParamFloat<px4::params::SV_SPEED_LIM>) _param_sv_speed_limit,
(ParamFloat<px4::params::SV_SPEED_TH>) _param_sv_speed_th
)
};