CA: remove legacy motor stopping logic

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

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp

@@ -83,21 +83,3 @@ 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) {
-
-			// Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy)
-			if (fabsf(actuator_sp(actuator_idx)) < .02f) {
-				_stopped_motors_mask |= motor_mask;
-
-			} else {
-				_stopped_motors_mask &= ~motor_mask;
-			}
-		}
-	}
-}

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -209,14 +209,6 @@ 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
-	 *
-	 * @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};
 	uint32_t _stopped_motors_mask{0};

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp

@@ -58,9 +58,3 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &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 ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
-{
-	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
-}

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp

@@ -45,9 +45,6 @@ public:
 
 	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 ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
-
 	const char *name() const override { return "Custom"; }
 
 protected:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -64,8 +64,6 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 		ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
 {
-	stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
-
 	// disable selected control surfaces during launch
 	launch_detection_status_s launch_detection_status;
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp

@@ -48,9 +48,3 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi
 	configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{});
 	return true;
 }
-
-void ActuatorEffectivenessRoverAckermann::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
-{
-	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
-}

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp

@@ -43,9 +43,6 @@ public:
 
 	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 ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
-
 	const char *name() const override { return "Rover (Ackermann)"; }
 private:
 	uint32_t _motors_mask{};

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -83,14 +83,6 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt,
 	}
 }
 
-void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
-{
-	if (matrix_index == 0) {
-		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
-	}
-}
-
 void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase)
 {
 	if (_flight_phase == flight_phase) {

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -75,9 +75,6 @@ public:
 
 	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 ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
-
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 private:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -88,14 +88,6 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d
 	}
 }
 
-void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
-{
-	if (matrix_index == 0) {
-		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
-	}
-}
-
 void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase)
 {
 	if (_flight_phase == flight_phase) {

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -72,10 +72,6 @@ public:
 
 	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 ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
-
-
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	const char *name() const override { return "VTOL Tailsitter"; }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -205,10 +205,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 				}
 			}
 		}
-
-		if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
-			stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp);
-		}
 	}
 }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp

@@ -54,9 +54,3 @@ bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configurati
 
 	return rotors_added_successfully;
 }
-
-void ActuatorEffectivenessUUV::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-		ActuatorVector &actuator_sp, const ActuatorVector &actuator_min, const ActuatorVector &actuator_max)
-{
-	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
-}

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp

@@ -54,9 +54,6 @@ public:
 		normalize[0] = true;
 	}
 
-	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index, ActuatorVector &actuator_sp,
-			    const ActuatorVector &actuator_min, const ActuatorVector &actuator_max) override;
-
 	const char *name() const override { return "UUV"; }
 
 protected: