mixer_module: send a last sample out after all outputs were disabled

This matters for PWM when the last output gets disabled on either FMU or IO
it would just keep on running.

Also when rebooting with a parameters reset or new airframe with no mapped outputs
it would previously keep outputting PWM with the disarmed value of the new airframe
e.g. 1000us which is a safety hazard because servos could break the physical limit of the
model or miscalibrated ESCs spinning motors.

.editorconfig

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

src/lib/mixer_module/mixer_module.hpp

@@ -288,6 +288,7 @@ 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,11 +191,15 @@ TEST_F(MixerModuleTest, basic)
 	mixing_output.setAllMaxValues(MAX_VALUE);
 	EXPECT_EQ(test_module.num_updates, 0);
 
-	// all functions disabled: not expected to get an update
+	// all functions disabled: expect to get one single update to process disabling the output signal
 	mixing_output.update();
 	mixing_output.updateSubscriptions(false);
 	mixing_output.update();
-	EXPECT_EQ(test_module.num_updates, 0);
+	EXPECT_EQ(test_module.num_updates, 1);
+	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));
-	_heading_error = matrix::wrap_pi(desired_heading - yaw);
+	float heading_error = matrix::wrap_pi(desired_heading - yaw);
 
 	if (_current_waypoint != current_waypoint) {
 		_currentState = GuidanceState::TURNING;
@@ -78,30 +78,21 @@ 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;
 
-			if (fabsf(_heading_error) < 0.05f) {
+		if (fabsf(heading_error) < 0.05f) {
 			_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;
-		}
 
 	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;
 		}
@@ -109,15 +100,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,
-				     dt) + _heading_error;
+	float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0,
+				     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,8 +124,6 @@ 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.
@@ -137,7 +135,6 @@ 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_MIN_TRN_VEL>) _param_rdd_min_turning_speed
+		(ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel
 	)
 };

src/modules/differential_drive/module.yaml

@@ -120,14 +120,3 @@ 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,17 +180,21 @@ mixer_tick()
 	 * Run the mixers.
 	 */
 	if (source == MIX_FAILSAFE) {
-		/* copy failsafe values to the servo outputs */
+		// Set failsafe value if the PWM output isn't disabled
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
+			if (r_page_servos[i] != 0) {
 				r_page_servos[i] = r_page_servo_failsafe[i];
 			}
+		}
 
 	} else if (source == MIX_DISARMED) {
-		/* copy disarmed values to the servo outputs */
+		// Set disarmed value if the PWM output isn't disabled
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
+			if (r_page_servos[i] != 0) {
 				r_page_servos[i] = r_page_servo_disarmed[i];
 			}
 		}
+	}
 
 	/* set arming */
 	bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm);