Default motor PWM configuration

boards/bitcraze/crazyflie/src/board_config.h

@@ -140,15 +140,7 @@
 
 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
-#define PWM_MOTOR_OFF	0
-#define PWM_SERVO_STOP	0
-#define PWM_DEFAULT_MIN 20
-#define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
-#define PWM_DEFAULT_MAX 255
-#define PWM_LOWEST_MAX 255
-#define PWM_DEFAULT_TRIM 1500
-
 
 /* High-resolution timer */
 #define HRT_TIMER		8	/* use timer8 for the HRT */

boards/bitcraze/crazyflie21/src/board_config.h

@@ -141,15 +141,7 @@
 
 #define PX4_PWM_ALTERNATE_RANGES
 #define PWM_LOWEST_MIN 0
-#define PWM_MOTOR_OFF	0
-#define PWM_SERVO_STOP	0
-#define PWM_DEFAULT_MIN 20
-#define PWM_HIGHEST_MIN 0
 #define PWM_HIGHEST_MAX 255
-#define PWM_DEFAULT_MAX 255
-#define PWM_LOWEST_MAX 255
-#define PWM_DEFAULT_TRIM 1500
-
 
 /* High-resolution timer */
 #define HRT_TIMER		8	/* use timer8 for the HRT */

src/drivers/drv_pwm_output.h

@@ -61,41 +61,11 @@ __BEGIN_DECLS
  */
 #define PWM_LOWEST_MIN 90
 
-/**
- * Default value for a shutdown motor
- */
-#define PWM_MOTOR_OFF	900
-
-/**
- * Default minimum PWM in us
- */
-#define PWM_DEFAULT_MIN 1000
-
-/**
- * Highest PWM allowed as the minimum PWM
- */
-#define PWM_HIGHEST_MIN 1600
-
 /**
  * Highest maximum PWM in us
  */
 #define PWM_HIGHEST_MAX 2500
 
-/**
- * Default maximum PWM in us
- */
-#define PWM_DEFAULT_MAX 2000
-
-/**
- * Default trim PWM in us
- */
-#define PWM_DEFAULT_TRIM 0
-
-/**
- * Lowest PWM allowed as the maximum PWM
- */
-#define PWM_LOWEST_MAX 200
-
 #endif // not PX4_PWM_ALTERNATE_RANGES
 
 /**

src/drivers/linux_pwm_out/module.yaml

@@ -4,7 +4,7 @@ actuator_output:
     - param_prefix: PWM_MAIN
       channel_label: 'Channel'
       standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
         min: { min: 800, max: 1400, default: 1000 }
         max: { min: 1600, max: 2200, default: 2000 }
         failsafe: { min: 800, max: 2200 }

src/drivers/pca9685_pwm_out/module.yaml

@@ -4,7 +4,7 @@ actuator_output:
     - param_prefix: PCA9685
       channel_label: 'Channel'
       standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
         min: { min: 800, max: 1400, default: 1000 }
         max: { min: 1600, max: 2200, default: 2000 }
         failsafe: { min: 800, max: 2200 }

src/drivers/pwm_out/PWMOut.cpp

@@ -218,42 +218,55 @@ void PWMOut::update_params()
 
 	updateParams();
 
-	// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
+	// Automatically set PWM configuration when a channel is first assigned
 	if (!_first_update_cycle) {
 		for (size_t i = 0; i < _num_outputs; i++) {
 			if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
 				int32_t output_function;
 
-				if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
-				    && output_function >= (int)OutputFunction::Servo1
-				    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
-					int32_t val = 1500;
-					PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
-					param_set(_mixing_output.disarmedParamHandle(i), &val);
+				if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) {
+					// Servos need PWM rate 50Hz and disramed value 1500us
+					if (output_function >= (int)OutputFunction::Servo1
+					    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
+						int32_t val = 1500;
+						PX4_INFO("Setting channel %i disarmed to %i", (int) val, i);
+						param_set(_mixing_output.disarmedParamHandle(i), &val);
 
-					// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
-					for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+						// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
+						for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
 
-						uint32_t channels = io_timer_get_group(timer);
+							uint32_t channels = io_timer_get_group(timer);
 
-						if ((channels & (1u << i)) == 0) {
-							continue;
-						}
+							if ((channels & (1u << i)) == 0) {
+								continue;
+							}
 
-						if ((channels & previously_set_functions) == 0) { // None of the channels was set
-							char param_name[17];
-							snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+							if ((channels & previously_set_functions) == 0) { // None of the channels was set
+								char param_name[17];
+								snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
 
-							int32_t tim_config = 0;
-							param_t handle = param_find(param_name);
+								int32_t tim_config = 0;
+								param_t handle = param_find(param_name);
 
-							if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
-								tim_config = 50;
-								PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
-								param_set(handle, &tim_config);
+								if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
+									tim_config = 50;
+									PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
+									param_set(handle, &tim_config);
+								}
 							}
 						}
 					}
+
+					// Motors need a minimum value that idles the motor and have a deadzone at the top of the range
+					if (output_function >= (int)OutputFunction::Motor1
+					    && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor
+						int32_t val = 1100;
+						PX4_INFO("Setting channel %i minimum to %i", (int) val, i);
+						param_set(_mixing_output.minParamHandle(i), &val);
+						val = 1900;
+						PX4_INFO("Setting channel %i maximum to %i", (int) val, i);
+						param_set(_mixing_output.maxParamHandle(i), &val);
+					}
 				}
 			}
 		}

src/drivers/pwm_out/module.yaml

@@ -5,7 +5,7 @@ actuator_output:
       param_prefix: '${PWM_MAIN_OR_AUX}'
       channel_labels: ['${PWM_MAIN_OR_AUX}', '${PWM_MAIN_OR_AUX_CAP}']
       standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
         min: { min: 800, max: 1400, default: 1000 }
         max: { min: 1600, max: 2200, default: 2000 }
         failsafe: { min: 800, max: 2200 }

src/drivers/px4io/module.yaml

@@ -7,7 +7,7 @@ actuator_output:
       channel_label_module_name_prefix: false
       timer_config_file: "boards/px4/io-v2/src/timer_config.cpp"
       standard_params:
-        disarmed: { min: 800, max: 2200, default: 900 }
+        disarmed: { min: 800, max: 2200, default: 1000 }
         min: { min: 800, max: 1400, default: 1000 }
         max: { min: 1600, max: 2200, default: 2000 }
         failsafe: { min: 800, max: 2200 }

src/drivers/px4io/px4io.cpp

@@ -702,36 +702,48 @@ void PX4IO::update_params()
 				if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
 					int32_t output_function;
 
-					if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
-					    && output_function >= (int)OutputFunction::Servo1
-					    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
-						int32_t val = 1500;
-						PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
-						param_set(_mixing_output.disarmedParamHandle(i), &val);
+					if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) {
+						if (output_function >= (int)OutputFunction::Servo1
+						    && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
+							int32_t val = 1500;
+							PX4_INFO("Setting channel %i disarmed to %i", (int) val, i);
+							param_set(_mixing_output.disarmedParamHandle(i), &val);
 
-						// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
-						for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) {
+							// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
+							for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) {
 
-							uint32_t channels = _group_channels[timer];
+								uint32_t channels = _group_channels[timer];
 
-							if ((channels & (1u << i)) == 0) {
-								continue;
-							}
+								if ((channels & (1u << i)) == 0) {
+									continue;
+								}
 
-							if ((channels & previously_set_functions) == 0) { // None of the channels was set
-								char param_name[17];
-								snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
+								if ((channels & previously_set_functions) == 0) { // None of the channels was set
+									char param_name[17];
+									snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
 
-								int32_t tim_config = 0;
-								param_t handle = param_find(param_name);
+									int32_t tim_config = 0;
+									param_t handle = param_find(param_name);
 
-								if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
-									tim_config = 50;
-									PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
-									param_set(handle, &tim_config);
+									if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
+										tim_config = 50;
+										PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
+										param_set(handle, &tim_config);
+									}
 								}
 							}
 						}
+
+						// Motors need a minimum value that idles the motor
+						if (output_function >= (int)OutputFunction::Motor1
+						    && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor
+							int32_t val = 1100;
+							PX4_INFO("Setting channel %i minimum to %i", (int) val, i);
+							param_set(_mixing_output.minParamHandle(i), &val);
+							val = 1900;
+							PX4_INFO("Setting channel %i maximum to %i", (int) val, i);
+							param_set(_mixing_output.maxParamHandle(i), &val);
+						}
 					}
 				}
 			}

src/lib/mixer_module/mixer_module.hpp

@@ -179,6 +179,8 @@ class MixingOutput : public ModuleParams
 
 	param_t functionParamHandle(int index) const { return _param_handles[index].function; }
 	param_t disarmedParamHandle(int index) const { return _param_handles[index].disarmed; }
+	param_t minParamHandle(int index) const { return _param_handles[index].min; }
+	param_t maxParamHandle(int index) const { return _param_handles[index].max; }
 
 	/**
 	 * Returns the actual failsafe value taking into account the assigned function

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp

@@ -84,3 +84,19 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const
 
 	return total_count;
 }
+
+void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp)
+{
+	for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) {
+		const uint32_t motor_mask = (1u << actuator_idx);
+
+		if (stoppable_motors_mask & motor_mask) {
+			if (fabsf(actuator_sp(actuator_idx)) < .02f) {
+				_stopped_motors_mask |= motor_mask;
+
+			} else {
+				_stopped_motors_mask &= ~motor_mask;
+			}
+		}
+	}
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -203,14 +203,23 @@ class ActuatorEffectiveness
 	/**
 	 * Get a bitmask of motors to be stopped
 	 */
-	virtual uint32_t getStoppedMotors() const { return 0; }
+	virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; }
 
 	/**
 	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
 	 * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
 	 */
 	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
 
+	/**
+	 * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is zero
+	 *
+	 * @param stoppable_motors_mask mask of motors that should be stopped if there's no thrust demand
+	 * @param actuator_sp outcome of the allocation to determine if the motor should be stopped
+	 */
+	virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp);
+
 protected:
-	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
+	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
+	uint32_t _stopped_motors_mask{0};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp

@@ -48,13 +48,20 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration
 		return false;
 	}
 
-	// motors
+	// Motors
 	_motors.enablePropellerTorque(false);
 	const bool motors_added_successfully = _motors.addActuators(configuration);
+	_motors_mask = _motors.getMotors();
 
 	// Torque
 	const bool torque_added_successfully = _torque.addActuators(configuration);
 
 	return (motors_added_successfully && torque_added_successfully);
 }
 
+void ActuatorEffectivenessCustom::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
+}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp

@@ -45,9 +45,15 @@ class ActuatorEffectivenessCustom : public ModuleParams, public ActuatorEffectiv
 
 	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
 	const char *name() const override { return "Custom"; }
 
 protected:
 	ActuatorEffectivenessRotors _motors;
 	ActuatorEffectivenessControlSurfaces _torque;
+
+	uint32_t _motors_mask{};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -53,6 +53,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 	// Motors
 	_rotors.enablePropellerTorque(false);
 	const bool rotors_added_successfully = _rotors.addActuators(configuration);
+	_forwards_motors_mask = _rotors.getForwardsMotors();
 
 	// Control Surfaces
 	_first_control_surface_idx = configuration.num_actuators_matrix[0];
@@ -61,6 +62,13 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 	return (rotors_added_successfully && surfaces_added_successfully);
 }
 
+void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
+}
+
 void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index,
 		ActuatorVector &actuator_sp)
 {

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -51,6 +51,10 @@ class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffec
 
 	void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override;
 
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
 private:
 	ActuatorEffectivenessRotors _rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
@@ -59,4 +63,6 @@ class ActuatorEffectivenessFixedWing : public ModuleParams, public ActuatorEffec
 	uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)};
 
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
+
+	uint32_t _forwards_motors_mask{};
 };