fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 07:28:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

differential: add slow down effect in mission mode

Browse Source
This commit is contained in:
chfriedrich98
2024-12-02 10:44:22 +01:00
committed by chfriedrich98
parent 8880569b31
commit 7e705bbf55
6 changed files with 30 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover
-1
View File
@@ -26,7 +26,6 @@ param set-default RD_MAX_THR_SPD 2.15
param set-default RD_SPEED_P 0.1
param set-default RD_SPEED_I 0.01
param set-default RD_MAX_YAW_RATE 180
param set-default RD_MISS_SPD_DEF 2
param set-default RD_TRANS_DRV_TRN 0.349066
param set-default RD_TRANS_TRN_DRV 0.174533
param set-default RD_MAX_YAW_ACCEL 1000
ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower
-1
View File
@@ -29,7 +29,6 @@ param set-default RD_MAX_SPEED 8
param set-default RD_YAW_P 5
param set-default RD_YAW_I 0.1
param set-default RD_MAX_YAW_RATE 30
param set-default RD_MISS_SPD_DEF 8
param set-default RD_TRANS_DRV_TRN 0.349066
param set-default RD_TRANS_TRN_DRV 0.174533
ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover
-1
View File
@@ -31,7 +31,6 @@ param set-default RD_MAX_THR_SPD 1.9
param set-default RD_MAX_THR_YAW_R 0.7
param set-default RD_MAX_YAW_ACCEL 600
param set-default RD_MAX_YAW_RATE 250
param set-default RD_MISS_SPD_DEF 1.5
param set-default RD_SPEED_P 0.1
param set-default RD_SPEED_I 0.01
param set-default RD_TRANS_DRV_TRN 0.785398
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp
+12 -2
View File
@@ -40,7 +40,7 @@ using namespace matrix;
RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent)
{
updateParams();
_max_forward_speed = _param_rd_miss_spd_def.get();
_max_forward_speed = _param_rd_max_speed.get();
_rover_differential_guidance_status_pub.advertise();
}
@@ -96,6 +96,16 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f
_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);
}
} else if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON
&& _param_rd_miss_spd_gain.get() > FLT_EPSILON) {
const float speed_reduction = math::constrain(_param_rd_miss_spd_gain.get() * math::interpolate(
M_PI_F - _waypoint_transition_angle, 0.f,
M_PI_F, 0.f, 1.f), 0.f, 1.f);
desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(),
_param_rd_max_decel.get(), distance_to_curr_wp, _max_forward_speed * (1.f - speed_reduction));
desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed,
_max_forward_speed);
}
} break;
@@ -219,6 +229,6 @@ void RoverDifferentialGuidance::updateWaypoints()
_max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get());
} else {
_max_forward_speed = _param_rd_miss_spd_def.get();
_max_forward_speed = _param_rd_max_speed.get();
}
}
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp
+2 -3
View File
@@ -144,9 +144,8 @@ private:
(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
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn,
(ParamFloat<px4::params::RD_MISS_SPD_GAIN>) _param_rd_miss_spd_gain
)
};
src/modules/rover_differential/module.yaml
+16 -11
View File
@@ -191,17 +191,6 @@ parameters:
decimal: 2
default: 2
RD_MISS_SPD_DEF:
description:
short: Default forward speed for the rover during auto modes
type: float
unit: m/s
min: 0
max: 100
increment: 0.01
decimal: 2
default: 1
RD_TRANS_TRN_DRV:
description:
short: Yaw error threshhold to switch from spot turning to driving
@@ -228,3 +217,19 @@ parameters:
increment: 0.01
decimal: 3
default: 0.174533
RD_MISS_SPD_GAIN:
description:
short: Tuning parameter for the speed reduction during waypoint transition
long: |
The waypoint transition speed is calculated as:
Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN)
The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP
interpolated from [0, 180] -> [0, 1].
Higher value -> More speed reduction during waypoint transitions.
type: float
min: 0.05
max: 100
increment: 0.01
decimal: 2
default: 1