fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 12:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

differential: add slew rates for setpoints, deprecate RD_MAN_YAW_SCALE and update airframe tuning

Browse Source
This commit is contained in:
chfriedrich98
2024-09-17 10:15:36 +02:00
committed by chfriedrich98
parent fbbfcdb7a6
commit 4f00df60ae
12 changed files with 300 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_differential/RoverDifferential.cpp
+18 -6
View File
@@ -63,7 +63,7 @@ void RoverDifferential::Run()
updateSubscriptions();
// Generate and publish attitude and velocity setpoints
// Generate and publish attitude, rate and speed setpoints
hrt_abstime timestamp = hrt_absolute_time();
switch (_nav_state) {
@@ -76,7 +76,19 @@ void RoverDifferential::Run()
rover_differential_setpoint.forward_speed_setpoint = NAN;
rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
rover_differential_setpoint.yaw_setpoint = NAN;
rover_differential_setpoint.yaw_rate_setpoint_normalized = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get();
if (_max_yaw_rate > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) {
const float scaled_yaw_rate_input = math::interpolate<float>(manual_control_setpoint.roll,
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
const float speed_diff = scaled_yaw_rate_input * _param_rd_wheel_track.get() / 2.f;
rover_differential_setpoint.speed_diff_setpoint_normalized = math::interpolate<float>(speed_diff,
-_param_rd_max_thr_yaw_r.get(), _param_rd_max_thr_yaw_r.get(), -1.f, 1.f);
} else {
rover_differential_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.roll;
}
rover_differential_setpoint.yaw_rate_setpoint = NAN;
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
}
@@ -92,7 +104,7 @@ void RoverDifferential::Run()
rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_setpoint = NAN;
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
}
@@ -109,7 +121,7 @@ void RoverDifferential::Run()
rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_setpoint = NAN;
if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
@@ -144,7 +156,7 @@ void RoverDifferential::Run()
rover_differential_setpoint.forward_speed_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_setpoint = NAN;
if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
@@ -191,7 +203,7 @@ void RoverDifferential::Run()
rover_differential_setpoint.forward_speed_setpoint = NAN;
rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f;
rover_differential_setpoint.yaw_rate_setpoint = NAN;
rover_differential_setpoint.yaw_rate_setpoint_normalized = 0.f;
rover_differential_setpoint.speed_diff_setpoint_normalized = 0.f;
rover_differential_setpoint.yaw_setpoint = NAN;
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
break;
src/modules/rover_differential/RoverDifferential.hpp
+5 -4
View File
@@ -129,9 +129,10 @@ private:
Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_MAN_YAW_SCALE>) _param_rd_man_yaw_scale,
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamFloat<px4::params::RD_MAX_THR_YAW_R>) _param_rd_max_thr_yaw_r,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
)
};
src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp
+137 -38
View File
@@ -51,6 +51,9 @@ void RoverDifferentialControl::updateParams()
{
ModuleParams::updateParams();
_max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F;
_max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F;
// Update PID
pid_set_parameters(&_pid_yaw_rate,
_param_rd_yaw_rate_p.get(), // Proportional gain
_param_rd_yaw_rate_i.get(), // Integral gain
@@ -69,6 +72,16 @@ void RoverDifferentialControl::updateParams()
0.f, // Derivative gain
_max_yaw_rate, // Integral limit
_max_yaw_rate); // Output limit
// Update slew rates
if (_max_yaw_rate > FLT_EPSILON) {
_yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate);
}
if (_max_yaw_accel > FLT_EPSILON) {
_yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel);
}
}
void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate,
@@ -84,63 +97,55 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
// Closed loop yaw control (Overrides yaw rate setpoint)
if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) {
float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw);
_yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint), dt);
_rover_differential_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState());
const float heading_error = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - vehicle_yaw);
_rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt);
_rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint;
} else {
_yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw);
}
// Yaw rate control
float speed_diff_normalized{0.f};
if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
if (_param_rd_wheel_track.get() > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward
const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _param_rd_wheel_track.get() /
2.f;
speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rd_max_thr_yaw_r.get(),
_param_rd_max_thr_yaw_r.get(), -1.f, 1.f);
}
speed_diff_normalized = math::constrain(speed_diff_normalized +
pid_calculate(&_pid_yaw_rate, _rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0, dt),
-1.f, 1.f); // Feedback
speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate,
_param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit,
_pid_yaw_rate, false);
} else { // Use normalized setpoint
speed_diff_normalized = PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint_normalized) ?
math::constrain(_rover_differential_setpoint.yaw_rate_setpoint_normalized, -1.f, 1.f) : 0.f;
speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.speed_diff_setpoint_normalized,
vehicle_yaw_rate,
_param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit,
_pid_yaw_rate, true);
}
// Speed control
float forward_speed_normalized{0.f};
if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control
if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
forward_speed_normalized = math::interpolate<float>(_rover_differential_setpoint.forward_speed_setpoint,
-_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(),
-1.f, 1.f);
}
if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) {
forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint,
vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle,
_param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, false);
forward_speed_normalized = math::constrain(forward_speed_normalized + pid_calculate(&_pid_throttle,
_rover_differential_setpoint.forward_speed_setpoint,
vehicle_forward_speed, 0,
dt), -1.f, 1.f); // Feedback
} else if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint
forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint_normalized,
vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle,
_param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, true);
} else { // Use normalized setpoint
forward_speed_normalized = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized) ?
math::constrain(_rover_differential_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
}
// Publish rover differential status (logging)
rover_differential_status_s rover_differential_status{};
rover_differential_status.timestamp = _timestamp;
rover_differential_status.actual_speed = vehicle_forward_speed;
rover_differential_status.actual_yaw = vehicle_yaw;
rover_differential_status.desired_yaw_rate = _rover_differential_setpoint.yaw_rate_setpoint;
rover_differential_status.actual_yaw_rate = vehicle_yaw_rate;
rover_differential_status.forward_speed_normalized = forward_speed_normalized;
rover_differential_status.speed_diff_normalized = speed_diff_normalized;
rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
rover_differential_status.pid_throttle_integral = _pid_throttle.integral;
rover_differential_status.pid_yaw_integral = _pid_yaw.integral;
_rover_differential_status_pub.publish(rover_differential_status);
_rover_differential_status.timestamp = _timestamp;
_rover_differential_status.measured_forward_speed = vehicle_forward_speed;
_rover_differential_status.measured_yaw = vehicle_yaw;
_rover_differential_status.measured_yaw_rate = vehicle_yaw_rate;
_rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral;
_rover_differential_status.pid_throttle_integral = _pid_throttle.integral;
_rover_differential_status.pid_yaw_integral = _pid_yaw.integral;
_rover_differential_status_pub.publish(_rover_differential_status);
// Publish to motors
actuator_motors_s actuator_motors{};
@@ -148,6 +153,100 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control);
actuator_motors.timestamp = _timestamp;
_actuator_motors_pub.publish(actuator_motors);
}
float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate,
const float max_thr_yaw_r,
const float max_yaw_accel, const float wheel_track, const float dt, SlewRate<float> &yaw_rate_with_accel_limit,
PID_t &pid_yaw_rate, const bool normalized)
{
float slew_rate_normalization{1.f};
if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized
slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f;
}
if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization);
yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt);
} else {
yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint);
}
// Transform yaw rate into speed difference
float speed_diff_normalized{0.f};
if (normalized) {
speed_diff_normalized = yaw_rate_with_accel_limit.getState();
} else {
_rover_differential_status.adjusted_yaw_rate_setpoint = yaw_rate_with_accel_limit.getState();
if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward
const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track /
2.f;
speed_diff_normalized = math::interpolate<float>(speed_diff, -max_thr_yaw_r,
max_thr_yaw_r, -1.f, 1.f);
}
speed_diff_normalized += pid_calculate(&pid_yaw_rate, yaw_rate_with_accel_limit.getState(), vehicle_yaw_rate, 0,
dt); // Feedback
}
return math::constrain(speed_diff_normalized, -1.f, 1.f);
}
float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint,
const float vehicle_forward_speed, const float max_thr_spd, SlewRate<float> &forward_speed_setpoint_with_accel_limit,
PID_t &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized)
{
float slew_rate_normalization{1.f};
if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized
slew_rate_normalization = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f;
}
// Apply acceleration and deceleration limit
if (fabsf(forward_speed_setpoint) >= fabsf(forward_speed_setpoint_with_accel_limit.getState())) {
if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
forward_speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization);
forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
} else {
forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
}
} else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
forward_speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization);
forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt);
} else {
forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint);
}
// Calculate normalized forward speed setpoint
float forward_speed_normalized{0.f};
if (normalized) {
forward_speed_normalized = _forward_speed_setpoint_with_accel_limit.getState();
} else { // Closed loop speed control
_rover_differential_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState();
if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
forward_speed_normalized = math::interpolate<float>(_forward_speed_setpoint_with_accel_limit.getState(),
-max_thr_spd, max_thr_spd,
-1.f, 1.f);
}
forward_speed_normalized += pid_calculate(&pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(),
vehicle_forward_speed, 0, dt); // Feedback
}
return math::constrain(forward_speed_normalized, -1.f, 1.f);
}
src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp
+57 -15
View File
@@ -43,7 +43,8 @@
#include <uORB/topics/rover_differential_setpoint.h>
#include <uORB/topics/rover_differential_status.h>
#include <uORB/topics/actuator_motors.h>
#include <lib/slew_rate/SlewRate.hpp>
#include <lib/slew_rate/SlewRateYaw.hpp>
// Standard libraries
#include <lib/pid/pid.h>
@@ -84,9 +85,42 @@ protected:
void updateParams() override;
private:
/**
* @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates.
* @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]].
* @param vehicle_yaw_rate Measured yaw rate [rad/s].
* @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s].
* @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2].
* @param wheel_track Wheel track [m].
* @param dt Time since last update [s].
* @param yaw_rate_with_accel_limit Yaw rate slew rate.
* @param pid_yaw_rate Yaw rate PID
* @param normalized Indicates if the forward speed setpoint is already normalized.
* @return Normalized speed differece setpoint with applied slew rates [-1, 1].
*/
float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel,
float wheel_track, float dt, SlewRate<float> &yaw_rate_with_accel_limit, PID_t &pid_yaw_rate, bool normalized);
/**
* @brief Compute normalized forward speed setpoint and apply slew rates.
* to the forward speed setpoint and doing closed loop speed control if enabled.
* @param forward_speed_setpoint Forward speed setpoint [m/s].
* @param vehicle_forward_speed Actual forward speed [m/s].
* @param max_thr_spd Speed the rover drives at maximum throttle [m/s].
* @param forward_speed_setpoint_with_accel_limit Speed slew rate.
* @param pid_throttle Throttle PID
* @param max_accel Maximum acceleration [m/s^2]
* @param max_decel Maximum deceleration [m/s^2]
* @param dt Time since last update [s].
* @param normalized Indicates if the forward speed setpoint is already normalized.
* @return Normalized forward speed setpoint with applied slew rates [-1, 1].
*/
float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float max_thr_spd,
SlewRate<float> &forward_speed_setpoint_with_accel_limit, PID_t &pid_throttle, float max_accel, float max_decel,
float dt, bool normalized);
/**
* @brief Compute normalized motor commands based on normalized setpoints.
*
* @param forward_speed_normalized Normalized forward speed [-1, 1].
* @param speed_diff_normalized Speed difference between left and right wheels [-1, 1].
* @return matrix::Vector2f Motor velocities for the right and left motors [-1, 1].
@@ -99,29 +133,37 @@ private:
// uORB publications
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<rover_differential_status_s> _rover_differential_status_pub{ORB_ID(rover_differential_status)};
rover_differential_status_s _rover_differential_status{};
// Variables
rover_differential_setpoint_s _rover_differential_setpoint{};
hrt_abstime _timestamp{0};
float _max_yaw_rate{0.f};
float _max_yaw_accel{0.f};
// Controllers
PID_t _pid_throttle; // The PID controller for the closed loop speed control
PID_t _pid_yaw; // The PID controller for the closed loop yaw control
PID_t _pid_yaw_rate; // The PID controller for the closed loop yaw rate control
PID_t _pid_throttle; // Closed loop speed control
PID_t _pid_yaw; // Closed loop yaw control
PID_t _pid_yaw_rate; // Closed loop yaw rate control
SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};
SlewRate<float> _yaw_rate_with_accel_limit{0.f};
SlewRateYaw<float> _yaw_setpoint_with_yaw_rate_limit;
// Parameters
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_MAX_THR_SPD>) _param_rd_max_thr_spd,
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_MAX_THR_SPD>) _param_rd_max_thr_spd,
(ParamFloat<px4::params::RD_MAX_ACCEL>) _param_rd_max_accel,
(ParamFloat<px4::params::RD_MAX_DECEL>) _param_rd_max_decel,
(ParamFloat<px4::params::RD_MAX_THR_YAW_R>) _param_rd_max_thr_yaw_r,
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamFloat<px4::params::RD_YAW_RATE_P>) _param_rd_yaw_rate_p,
(ParamFloat<px4::params::RD_YAW_RATE_I>) _param_rd_yaw_rate_i,
(ParamFloat<px4::params::RD_SPEED_P>) _param_rd_p_gain_speed,
(ParamFloat<px4::params::RD_SPEED_I>) _param_rd_i_gain_speed,
(ParamFloat<px4::params::RD_YAW_P>) _param_rd_p_gain_yaw,
(ParamFloat<px4::params::RD_YAW_I>) _param_rd_i_gain_yaw,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamFloat<px4::params::RD_MAX_YAW_ACCEL>) _param_rd_max_yaw_accel,
(ParamFloat<px4::params::RD_YAW_RATE_P>) _param_rd_yaw_rate_p,
(ParamFloat<px4::params::RD_YAW_RATE_I>) _param_rd_yaw_rate_i,
(ParamFloat<px4::params::RD_SPEED_P>) _param_rd_p_gain_speed,
(ParamFloat<px4::params::RD_SPEED_I>) _param_rd_i_gain_speed,
(ParamFloat<px4::params::RD_YAW_P>) _param_rd_p_gain_yaw,
(ParamFloat<px4::params::RD_YAW_I>) _param_rd_i_gain_yaw,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp
+3 -3
View File
@@ -91,9 +91,9 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f
desired_forward_speed = _max_forward_speed;
if (_waypoint_transition_angle < M_PI_F - _param_rd_trans_drv_trn.get()) {
if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) {
if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON) {
desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(),
_param_rd_max_accel.get(), distance_to_curr_wp, 0.0f);
_param_rd_max_decel.get(), distance_to_curr_wp, 0.0f);
desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed);
}
}
@@ -130,7 +130,7 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f
rover_differential_setpoint.forward_speed_setpoint = desired_forward_speed;
rover_differential_setpoint.forward_speed_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_rate_setpoint = NAN;
rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
rover_differential_setpoint.yaw_setpoint = desired_yaw;
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
}
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp
+5 -5
View File
@@ -140,11 +140,11 @@ private:
// Parameters
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RD_MAX_JERK>) _param_rd_max_jerk,
(ParamFloat<px4::params::RD_MAX_ACCEL>) _param_rd_max_accel,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::RD_MISS_SPD_DEF>) _param_rd_miss_spd_def,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RD_MAX_JERK>) _param_rd_max_jerk,
(ParamFloat<px4::params::RD_MAX_DECEL>) _param_rd_max_decel,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::RD_MISS_SPD_DEF>) _param_rd_miss_spd_def,
(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn
src/modules/rover_differential/module.yaml
+31 -13
View File
@@ -16,19 +16,6 @@ parameters:
decimal: 3
default: 0.5
RD_MAN_YAW_SCALE:
description:
short: Manual yaw rate scale
long: |
In manual mode the setpoint for the yaw rate received from the yaw stick
is scaled by this value.
type: float
min: 0.001
max: 1
increment: 0.01
decimal: 3
default: 1
RD_YAW_P:
description:
short: Proportional gain for closed loop yaw controller
@@ -113,6 +100,21 @@ parameters:
decimal: 2
default: 0.5
RD_MAX_DECEL:
description:
short: Maximum deceleration
long: |
Maximum decelaration is used to limit the deceleration of the rover.
Set to -1 to disable, causing the rover to decelerate as fast as possible.
Caution: This disables the slow down effect in auto modes.
type: float
unit: m/s^2
min: -1
max: 100
increment: 0.01
decimal: 2
default: -1
RD_MAX_SPEED:
description:
short: Maximum speed setpoint
@@ -156,6 +158,22 @@ parameters:
decimal: 2
default: 90
RD_MAX_YAW_ACCEL:
description:
short: Maximum allowed yaw acceleration for the rover
long: |
This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints
to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change.
This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track.
Set to -1 to disable.
type: float
unit: deg/s^2
min: -1
max: 1000
increment: 0.01
decimal: 2
default: -1
RD_MAX_THR_YAW_R:
description:
short: Yaw rate turning left/right wheels at max speed in opposite directions