fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 13:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Differential Rover: Refactor and clean up, align with Ackermann rover(#24318)

Browse Source 
* differential: refactor code architecture

* Offboard fix

* fix accel/decel slew rate
This commit is contained in:
chfriedrich98
2025-02-18 18:26:38 +01:00
committed by GitHub
parent 3c129aefa1
commit 04a3c4af20
29 changed files with 1722 additions and 1362 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/rover_control/RoverControl.cpp
+72 -17
View File
@@ -48,26 +48,24 @@ float throttleControl(SlewRate<float> &motor_setpoint, const float throttle_setp
if (accelerating && max_accel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Acceleration slew rate
motor_setpoint.setSlewRate(max_accel / max_thr_spd);
motor_setpoint.update(throttle_setpoint, dt);
// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint -
current_motor_setpoint)) {
motor_setpoint.setForcedValue(current_motor_setpoint);
motor_setpoint.setForcedValue(throttle_setpoint);
}
motor_setpoint.update(throttle_setpoint, dt);
} else if (!accelerating && max_decel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Deceleration slew rate
motor_setpoint.setSlewRate(max_decel / max_thr_spd);
motor_setpoint.update(throttle_setpoint, dt);
// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint -
current_motor_setpoint)) {
motor_setpoint.setForcedValue(current_motor_setpoint);
motor_setpoint.setForcedValue(throttle_setpoint);
}
motor_setpoint.update(throttle_setpoint, dt);
} else { // Fallthrough if slew rate parameters are not configured
motor_setpoint.setForcedValue(throttle_setpoint);
}
@@ -88,14 +86,13 @@ float attitudeControl(SlewRateYaw<float> &adjusted_yaw_setpoint, PID &pid_yaw,
if (yaw_slew_rate > FLT_EPSILON) { // Apply slew rate if configured
adjusted_yaw_setpoint.setSlewRate(yaw_slew_rate);
adjusted_yaw_setpoint.update(yaw_setpoint, dt);
// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
if (fabsf(wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)) > fabsf(wrap_pi(yaw_setpoint - vehicle_yaw))) {
adjusted_yaw_setpoint.setForcedValue(vehicle_yaw);
adjusted_yaw_setpoint.setForcedValue(yaw_setpoint);
}
adjusted_yaw_setpoint.update(yaw_setpoint, dt);
} else {
adjusted_yaw_setpoint.setForcedValue(yaw_setpoint);
}
@@ -118,26 +115,24 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, const
// Apply acceleration and deceleration limit
if (fabsf(speed_setpoint) >= fabsf(vehicle_speed) && max_accel > FLT_EPSILON) {
speed_with_rate_limit.setSlewRate(max_accel);
speed_with_rate_limit.update(speed_setpoint, dt);
// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf(
speed_setpoint - vehicle_speed)) {
speed_with_rate_limit.setForcedValue(vehicle_speed);
speed_with_rate_limit.setForcedValue(speed_setpoint);
}
speed_with_rate_limit.update(speed_setpoint, dt);
} else if (fabsf(speed_setpoint) < fabsf(vehicle_speed) && max_decel > FLT_EPSILON) {
speed_with_rate_limit.setSlewRate(max_decel);
speed_with_rate_limit.update(speed_setpoint, dt);
// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf(
speed_setpoint - vehicle_speed)) {
speed_with_rate_limit.setForcedValue(vehicle_speed);
speed_with_rate_limit.setForcedValue(speed_setpoint);
}
speed_with_rate_limit.update(speed_setpoint, dt);
} else { // Fallthrough if slew rate is not configured
speed_with_rate_limit.setForcedValue(speed_setpoint);
}
@@ -153,12 +148,72 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, const
}
// Feedback control
pid_speed.setSetpoint(speed_with_rate_limit.getState());
forward_speed_normalized += pid_speed.update(vehicle_speed, dt);
if (fabsf(speed_with_rate_limit.getState()) > FLT_EPSILON) {
pid_speed.setSetpoint(speed_with_rate_limit.getState());
forward_speed_normalized += pid_speed.update(vehicle_speed, dt);
} else {
pid_speed.resetIntegral();
}
return math::constrain(forward_speed_normalized, -1.f, 1.f);
}
float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, const float yaw_rate_setpoint,
const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float max_yaw_decel,
const float wheel_track, const float dt)
{
// Apply acceleration and deceleration limit
if (fabsf(yaw_rate_setpoint) >= fabsf(vehicle_yaw_rate) && max_yaw_accel > FLT_EPSILON) {
adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_accel);
adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt);
// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf(
yaw_rate_setpoint - vehicle_yaw_rate)) {
adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint);
}
} else if (fabsf(yaw_rate_setpoint) < fabsf(vehicle_yaw_rate) && max_yaw_decel > FLT_EPSILON) {
adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_decel);
adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt);
// Reinitialize slew rate if current value is closer to setpoint than post slew rate value
if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf(
yaw_rate_setpoint - vehicle_yaw_rate)) {
adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint);
}
} else { // Fallthrough if slew rate is not configured
adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint);
}
// Transform yaw rate into speed difference
float speed_diff_normalized{0.f};
if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward
const float speed_diff = adjusted_yaw_rate_setpoint.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);
}
// Feedback control
if (fabsf(adjusted_yaw_rate_setpoint.getState()) > FLT_EPSILON) {
pid_yaw_rate.setSetpoint(adjusted_yaw_rate_setpoint.getState());
speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt);
} else {
pid_yaw_rate.resetIntegral();
}
return math::constrain(speed_diff_normalized, -1.f, 1.f);
}
void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned,
position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos,
MapProjection &global_ned_proj_ref)
src/lib/rover_control/RoverControl.hpp
+19
View File
@@ -92,6 +92,25 @@ float attitudeControl(SlewRateYaw<float> &adjusted_yaw_setpoint, PID &pid_yaw, f
float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, float speed_setpoint,
float vehicle_speed, float max_accel, float max_decel, float max_thr_speed, float dt);
/**
* Applies yaw acceleration slew rate to a yaw rate setpoint and calculates the necessary speed diff setpoint
* using a feedforward term and/or a PID controller.
* Note: This function is only for rovers that control the rate through a speed difference between the left/right wheels.
* @param adjusted_yaw_rate_setpoint Yaw rate setpoint with applied slew rate [-1, 1] (Updated by this function).
* @param pid_yaw_rate Yaw rate PID (Updated by this function).
* @param yaw_rate_setpoint Yaw rate setpoint [rad/s].
* @param vehicle_yaw_rate Measured vehicle 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 [rad/s^2].
* @param max_yaw_decel Maximum allowed yaw deceleration [rad/s^2].
* @param wheel_track Distance from the center of the right wheel to the center of the left wheel [m].
* @param dt Time since last update [s].
* @return Normalized speed difference setpoint [-1, 1].
*/
float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate,
float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, float max_yaw_decel,
float wheel_track, float dt);
/**
* Projects positionSetpointTriplet waypoints from global to ned frame.
* @param curr_wp_ned Current waypoint in NED frame (Updated by this function)
src/lib/rover_control/module.yaml
+17 -1
View File
@@ -133,7 +133,23 @@ parameters:
RO_YAW_ACCEL_LIM:
description:
short: Yaw acceleration limit
long: Set to -1 to disable.
long: |
Used to cap how quickly the magnitude of yaw rate setpoints can increase.
Set to -1 to disable.
type: float
unit: deg/s^2
min: -1
max: 10000
increment: 0.01
decimal: 2
default: -1
RO_YAW_DECEL_LIM:
description:
short: Yaw deceleration limit
long: |
Used to cap how quickly the magnitude of yaw rate setpoints can decrease.
Set to -1 to disable.
type: float
unit: deg/s^2
min: -1