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

.editorconfig

@@ -1,14 +1,6 @@
 root = true
 
-[*]
+[*.{c,cpp,cc,h,hpp}]
-insert_final_newline = false
-
-[{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
 indent_style = tab
 tab_width = 8
-# Not in the official standard, but supported by many editors
 max_line_length = 120
-
-[*.yaml]
-indent_style = space
-indent_size = 2

src/drivers/px4io/px4io.cpp

@@ -364,13 +364,6 @@ PX4IO::~PX4IO()
 bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			  unsigned num_outputs, unsigned num_control_groups_updated)
 {
-	for (size_t i = 0; i < num_outputs; i++) {
-		if (!_mixing_output.isFunctionSet(i)) {
-			// do not run any signal on disabled channels
-			outputs[i] = 0;
-		}
-	}
-
 	if (!_test_fmu_fail) {
 		/* output to the servos */
 		io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);

src/lib/mixer_module/mixer_module.cpp

@@ -455,8 +455,7 @@ bool MixingOutput::update()
 		}
 	}
 
-	// Send output if any function mapped or one last disabling sample
+	if (!all_disabled) {
-	if (!all_disabled || !_was_all_disabled) {
 		if (!_armed.armed && !_armed.manual_lockdown) {
 			_actuator_test.overrideValues(outputs, _max_num_outputs);
 		}
@@ -464,8 +463,6 @@ bool MixingOutput::update()
 		limitAndUpdateOutputs(outputs, has_updates);
 	}
 
-	_was_all_disabled = all_disabled;
-
 	return true;
 }
 

src/lib/mixer_module/mixer_module.hpp

@@ -288,7 +288,6 @@ private:
 	hrt_abstime _lowrate_schedule_interval{300_ms};
 	ActuatorTest _actuator_test{_function_assignment};
 	uint32_t _reversible_mask{0}; ///< per-output bits. If set, the output is configured to be reversible (motors only)
-	bool _was_all_disabled{false};
 
 	uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback
 

src/lib/mixer_module/mixer_module_tests.cpp

@@ -191,15 +191,11 @@ TEST_F(MixerModuleTest, basic)
 	mixing_output.setAllMaxValues(MAX_VALUE);
 	EXPECT_EQ(test_module.num_updates, 0);
 
-	// all functions disabled: expect to get one single update to process disabling the output signal
+	// all functions disabled: not expected to get an update
 	mixing_output.update();
 	mixing_output.updateSubscriptions(false);
 	mixing_output.update();
-	EXPECT_EQ(test_module.num_updates, 1);
+	EXPECT_EQ(test_module.num_updates, 0);
-	mixing_output.update();
-	mixing_output.updateSubscriptions(false);
-	mixing_output.update();
-	EXPECT_EQ(test_module.num_updates, 1);
 	test_module.reset();
 
 	// configure motor, ensure all still disarmed

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

src/modules/px4iofirmware/mixer.cpp

@@ -180,19 +180,15 @@ mixer_tick()
 	 * Run the mixers.
 	 */
 	if (source == MIX_FAILSAFE) {
-		// Set failsafe value if the PWM output isn't disabled
+		/* copy failsafe values to the servo outputs */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			if (r_page_servos[i] != 0) {
+			r_page_servos[i] = r_page_servo_failsafe[i];
-				r_page_servos[i] = r_page_servo_failsafe[i];
-			}
 		}
 
 	} else if (source == MIX_DISARMED) {
-		// Set disarmed value if the PWM output isn't disabled
+		/* copy disarmed values to the servo outputs */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
-			if (r_page_servos[i] != 0) {
+			r_page_servos[i] = r_page_servo_disarmed[i];
-				r_page_servos[i] = r_page_servo_disarmed[i];
-			}
 		}
 	}