From 6b7aed3d44e76ebfca674ec91cd187eb434c91dd Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Wed, 3 May 2023 15:16:42 +0200
Subject: [PATCH] Boats: stop motors when commanding zero speed

---
 .../ActuatorEffectivenessCustom.cpp                      | 9 ++++++++-
 .../ActuatorEffectivenessCustom.hpp                      | 6 ++++++
 2 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp
index ca991b5424..ead2043682 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp
@@ -48,9 +48,10 @@ 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);
@@ -58,3 +59,9 @@ 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 matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
+}
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp
index e06278f08f..0cc1390bd2 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp
@@ -45,9 +45,15 @@ 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 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{};
 };