Allocation: expose threshold to decare motor stopped in new param CA_STOP_MOT_THLD

Instead of outputting a command to minimally rotate a motor, it is desirable
to competely stop it if the commanded setpoints is below some value.
E.g. a tiltable motor on a tiltrotor VTOL should be stopped if the commanded
thrust is 0 in fixed-wing mode, but not stopped when in hover (thus PWM_MIN has
to be configured to spin the motor).
This commit replaces the previous hard-coded limit of 2% motor setpoint with
the new parameter CA_STOP_MOT_THLD.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp

@@ -84,15 +84,16 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const
 	return total_count;
 }
 
-void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp)
+void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp,
+		float threshold)
 {
 	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) {
 
-			// Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy)
-			if (fabsf(actuator_sp(actuator_idx)) < .02f) {
+			// Stop motor if its setpoint is below threshold.
+			if (fabsf(actuator_sp(actuator_idx)) < threshold) {
 				_stopped_motors_mask |= motor_mask;
 
 			} else {

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -198,7 +198,7 @@ public:
 	 */
 	virtual 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) {}
+				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold) {}
 
 	/**
 	 * Get a bitmask of motors to be stopped
@@ -212,12 +212,14 @@ public:
 	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
+	 * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is below threshold
 	 *
 	 * @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
+	 * @param threshold actuator setpoint threshold below which the motor is stopped
 	 */
-	virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp);
+	virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp,
+			float threshold);
 
 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};

src/modules/control_allocator/ControlAllocator.cpp

@@ -445,7 +445,7 @@ ControlAllocator::Run()
 			_control_allocation[i]->allocate();
 			_actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers
 			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
-								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
+								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax(), _param_ca_stop_mot_thld.get());
 
 			if (_has_slew_rate) {
 				_control_allocation[i]->applySlewRateLimit(dt);

src/modules/control_allocator/ControlAllocator.hpp

@@ -216,7 +216,8 @@ private:
 		(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
 		(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
 		(ParamInt<px4::params::CA_FAILURE_MODE>) _param_ca_failure_mode,
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
+		(ParamFloat<px4::params::CA_STOP_MOT_THLD>) _param_ca_stop_mot_thld
 	)
 
 };

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp

@@ -61,7 +61,7 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration
 
 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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold)
 {
-	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
+	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp, stop_threshold);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp

@@ -47,7 +47,7 @@ public:
 
 	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 matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold) override;
 
 	const char *name() const override { return "Custom"; }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -63,9 +63,9 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 
 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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold)
 {
-	stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
+	stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp, stop_threshold);
 }
 
 void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index,

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -53,7 +53,7 @@ public:
 
 	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 matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold) override;
 
 private:
 	ActuatorEffectivenessRotors _rotors;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -145,7 +145,7 @@ bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &conf
 
 void ActuatorEffectivenessHelicopter::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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold)
 {
 	_saturation_flags = {};
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp

@@ -83,7 +83,7 @@ public:
 
 	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 matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold) override;
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp

@@ -103,7 +103,7 @@ bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuratio
 
 void ActuatorEffectivenessHelicopterCoaxial::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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold)
 {
 	_saturation_flags = {};
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp

@@ -71,7 +71,7 @@ public:
 
 	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 matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold) override;
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp

@@ -64,9 +64,10 @@ TEST(ActuatorEffectivenessHelicopterTest, ThrottleCurve)
 	actuator_min.setAll(0.f);
 	ActuatorEffectiveness::ActuatorVector actuator_max{};
 	actuator_max.setAll(1.f);
+	const float stop_threshold = 0.f; // do not stop motors
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.1f;
-	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max, 0.f);
 	EXPECT_FLOAT_EQ(actuator_sp(0), 0.f);
 
 	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.f;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp

@@ -78,7 +78,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
 
 void ActuatorEffectivenessMCTilt::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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold)
 {
 	actuator_sp += _tilt_offsets;
 	// TODO: dynamic matrix update

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp

@@ -57,7 +57,7 @@ public:
 
 	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 matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold) override;
 
 	const char *name() const override { return "MC Tilt"; }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp

@@ -51,7 +51,7 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi
 
 void ActuatorEffectivenessRoverAckermann::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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold)
 {
-	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
+	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp, stop_threshold);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp

@@ -45,7 +45,7 @@ public:
 
 	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 matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold) override;
 
 	const char *name() const override { return "Rover (Ackermann)"; }
 private:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -85,10 +85,10 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt,
 
 void ActuatorEffectivenessStandardVTOL::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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold)
 {
 	if (matrix_index == 0) {
-		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
+		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp, stop_threshold);
 	}
 }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -77,7 +77,7 @@ public:
 
 	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 matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold) override;
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -90,10 +90,10 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d
 
 void ActuatorEffectivenessTailsitterVTOL::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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold)
 {
 	if (matrix_index == 0) {
-		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
+		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp, stop_threshold);
 	}
 }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -74,7 +74,7 @@ public:
 
 	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 matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold) override;
 
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -126,7 +126,7 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt
 
 void ActuatorEffectivenessTiltrotorVTOL::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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold)
 {
 	// apply tilt
 	if (matrix_index == 0) {
@@ -208,7 +208,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 		}
 
 		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
-			stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);
+			stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp, stop_threshold);
 		}
 	}
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -82,7 +82,7 @@ public:
 
 	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 matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold) override;
 
 	const char *name() const override { return "VTOL Tiltrotor"; }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp

@@ -57,7 +57,7 @@ bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configurati
 
 void ActuatorEffectivenessUUV::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)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold)
 {
-	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
+	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp, stop_threshold);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp

@@ -56,7 +56,7 @@ public:
 
 	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 matrix::Vector<float, NUM_ACTUATORS> &actuator_max, float stop_threshold) override;
 
 	const char *name() const override { return "UUV"; }
 

src/modules/control_allocator/module.yaml

@@ -592,6 +592,20 @@ parameters:
                 1: Remove first failed motor from effectiveness
             default: 0
 
+        CA_STOP_MOT_THLD:
+            description:
+                short: Threshold on absolute thrust setpoint to stop stoppable motors.
+                long: |
+                    Stop motor (send NAN) if absolute actuator setpoint is below this threshold on
+                    a stoppable motor (e.g. fixed-wing motors).
+                    Set to 0 to not stop any motor.
+            type: float
+            decimal: 2
+            increment: 0.01
+            min: 0
+            max: 1
+            default: 0.02
+
 # Mixer
 mixer:
     actuator_types: