Differential Drive: Improved logic, to be independent of the heading gain

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp

@@ -64,7 +64,7 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 
 	float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0),
 				current_waypoint(1));
-	float heading_error = matrix::wrap_pi(desired_heading - yaw);
+	_heading_error = matrix::wrap_pi(desired_heading - yaw);
 
 	if (_current_waypoint != current_waypoint) {
 		_currentState = GuidanceState::TURNING;
@@ -78,21 +78,30 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 	float desired_speed = 0.f;
 
 	switch (_currentState) {
-	case GuidanceState::TURNING:
+	case GuidanceState::TURNING: {
-		desired_speed = 0.f;
+			desired_speed = 0.f;
 
-		if (fabsf(heading_error) < 0.05f) {
+			if (fabsf(_heading_error) < 0.05f) {
-			_currentState = GuidanceState::DRIVING;
+				_currentState = GuidanceState::DRIVING;
+			}
+
+			float minTurningSpeed = _param_rdd_min_turning_speed.get();
+			float headingPGain = _param_rdd_p_gain_heading.get();
+
+			// Make sure we do not get stuck while turning as the error gets too small
+			if ((fabsf(_heading_error) < minTurningSpeed) && (headingPGain > 0.01f)) {
+				_heading_error = (_heading_error > 0) ? minTurningSpeed * 1 / headingPGain : -minTurningSpeed * 1 / headingPGain;
+			}
+
+			break;
 		}
 
-		break;
-
 	case GuidanceState::DRIVING: {
 			const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
 						   _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
 			_forwards_velocity_smoothing.updateDurations(max_velocity);
 			_forwards_velocity_smoothing.updateTraj(dt);
-			desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.8f,
+			desired_speed = math::interpolate<float>(abs(_heading_error), 0.1f, 0.8f,
 					_forwards_velocity_smoothing.getCurrentVelocity(), 0.0f);
 			break;
 		}
@@ -100,15 +109,15 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 	case GuidanceState::GOAL_REACHED:
 		// temporary till I find a better way to stop the vehicle
 		desired_speed = 0.f;
-		heading_error = 0.f;
+		_heading_error = 0.f;
 		angular_velocity = 0.f;
 		_desired_angular_velocity = 0.f;
 		break;
 	}
 
 	// Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint.
-	float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0,
+	float angular_velocity_pid = pid_calculate(&_heading_p_controller, _heading_error, angular_velocity, 0,
-				     dt) + heading_error;
+				     dt) + _heading_error;
 
 	differential_drive_setpoint_s output{};
 	output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);

src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp

@@ -124,6 +124,8 @@ private:
 	float _max_speed; ///< The maximum speed.
 	float _max_angular_velocity; ///< The maximum angular velocity.
 
+	float _heading_error{0.0f};
+
 	matrix::Vector2d _current_waypoint; ///< The current waypoint.
 
 	VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion.
@@ -135,6 +137,7 @@ private:
 		(ParamFloat<px4::params::RDD_P_HEADING>) _param_rdd_p_gain_heading,
 		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
 		(ParamFloat<px4::params::RDD_MAX_JERK>) _param_rdd_max_jerk,
-		(ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel
+		(ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel,
+		(ParamFloat<px4::params::RDD_MIN_TRN_VEL>) _param_rdd_min_turning_speed
 	)
 };

src/modules/differential_drive/module.yaml

@@ -120,3 +120,14 @@ parameters:
         increment: 0.01
         decimal: 2
         default: 0.5
+      RDD_MIN_TRN_VEL:
+        description:
+          short: Minimun turning speed
+          long: Minimum turning speed required to ensure vehicle turns properly, preventing no-turn situations caused by minor heading errors or motor limitations at low speeds.
+        type: float
+        unit: rad/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.2