differential: add slow down effect in mission mode

PX4 · Nov 25, 2024 · 62afec1 · 62afec1
1 parent ce58581
commit 62afec1
Show file tree

Hide file tree

Showing 6 changed files with 30 additions and 19 deletions.
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover
@@ -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

diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower
@@ -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
 

diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover
@@ -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

diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp
@@ -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();
 	}
 }
diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp
@@ -144,9 +144,8 @@ class RoverDifferentialGuidance : public ModuleParams
 		(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
 	)
 };
diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml
@@ -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